NAVAL  POSTGRADUATE  SCHOOL 
MONTEREY,  CALIFORNIA 


SENSOR  BASED  NAVIGATION  AND 
LOCALIZATION  METHODS  FOR 
AUTONOMOUS  UNDERWATER  VEHICLES 

by 

Kevin  D.  Conowitch 
June,  1995 

Thesis  Advisor:  Roberto  Cristi 

Approved  for  public  release;  distribution  is  unlimited. 

[Iftic  QtJALiry  mSPBCIED  9 


REPORT  DOCUMENTATION  PAGE 

Form  Approved  OMB  No.  0704-0188 

Public  reporting  burden  for  this  collection  of  information  is  estimated  to  average  1  hour  per  response,  including  the  time  for  reviewing  instruction,  searching  existing  data 
sources,  gathering  and  maintaining  the  data  needed,  and  completing  and  reviewing  the  collection  of  information.  Send  comments  regarding  this  burden  estimate  or  any 
other  aspect  of  this  collection  of  information,  including  suggestions  for  reducing  this  burden,  to  Washington  Headquarters  Services,  Directorate  for  Information  Operations 
and  Reports,  1215  Jefferson  Davis  Highway,  Suite  1204,  Arlington,  VA  22202-4302,  and  to  the  Office  of  Management  and  Budget,  Paperwork  Reduction  Project  (0704- 
0188)  Washington  DC  20503. 

1.  AGENCY  USE  ONLY  (leave  Wowi;  2.  REPORT  DATE  3.  REPORT  TYPE  AND  DATES  COVERED 

June  1995  Master’s  Thesis 

4.  TITLE  AND  SUBTITLE  SENSOR  BASED  NAVIGATION  AND 

LOCALIZATION  METHODS  FOR  AUTONOMOUS  UNDERWATER 
VEHICLES 

5.  FUNDING  NUMBERS 

6.  AUTHOR(S)  Kevin  D.  Conowitch 

7.  PERFORMING  ORGANIZATION  NAME(S)  AND  ADDRESS(ES) 

Naval  Postgraduate  School 

Monterey  CA  93943-5000 

8.  PERFORMING 

ORGANIZATION 

REPORT  NUMBER 

9,  SPONSORING/MONITORING  AGENCY  NAME(S)  AND  ADDRESS(ES) 

10,  SPONSORING/MONITORING 
AGENCY  REPORT  NUMBER 

1 1 .  SUPPLEMENTARY  NOTES  The  views  expressed  in  this  thesis  are  those  of  the  author  and  do  not  reflect  the 
official  policy  or  position  of  the  Department  of  Defense  or  the  U.S.  Govenunent. 

12a.  DISTRIBUnON/AVAILABILnY  STATEMENT 

Approved  for  public  release;  distribution  is  unlimited. 

12b.  DISTRIBUTION  CODE 

13.  ABSTRACT  (maximum  200  words) 

An  algorithm  designed  to  navigate  an  Autonomous  Underwater  Vehicle  (AUV)  within  a  charted 
environment  is  presented.  The  algorithm  processes  sensor  inputs  from  the  AUV  high  resolution  scanning  sonar, 
compass  and  velocimeter.  The  operating  environment  is  modeled  with  a  suitable  three-dimensional  potential 
function  and  its  gradient  which  form  an  attractive  field.  This  algorithm  provides  performance  comparable  to  the 
Kalman  Filter  with  the  advantage  of  reduced  computational  requirements  facilitated  by  pre-computation  and 
table  look-up  of  correction  factors.  Applications  of  data  smoothing  filters  and  sliding  mode  theory  are  also 
investigated.  The  applicability  and  robustness  of  this  approach  are  demonstrated  with  actual  test  data  obtained 
with  the  NFS  Phoenix  submersible  and  extended  simulation  of  complex  environments  including  uiunodeled 
obstacles. 

14.  SUBJECT  TERMS  Autonomous  Underwater  Vehicle,  estimation,  navigation, 
potential  functions,  sliding  mode  observer 

15.  NUMBER  OF 

PAGES  104 

16.  PRICE  CODE 

17.  SECURITY  CLASSffl- 
CATTON  OF  REPORT 
Unclassified 

18.  SECURITY  CLASSM- 
CATION  OF  THIS  PAGE 
Unclassified 

19.  SECURITY  CLASSmCA- 
HON  OF  ABSTRACT 
Unclassified 

20.  LIMITATION  OF 
ABSTRACT 

UL 

NSN  7540-0 1  -280-5500  Standard  Form  298  (Rev.  2-89) 


Prescribed  by  ANSI  Std.  239-18  298-102 


1 


Approved  for  public  release;  distribution  is  unlimited. 


SENSOR  BASED  NAVIGATION  AND 
LOCALIZATION  METHODS  FOR 
AUTONOMOUS  UNDERWATER  VEHICLES 


Kevin  D.  Cono witch 

Lieutenant  Commander,  United  States  Navy 
B.S.,  University  of  California  at  Los  Angeles,  1984 

Submitted  in  partial  fulfillment 
of  the  requirements  for  the  degree  of 

MASTER  OF  SCIENCE  IN  ELECTRICAL  ENGINEERING 

from  the 


Author: 


Approved  by; 


NAVAL  POSTGRADUATE  SCHOOL 


Robert  G.  Hutchins,  Second  Reader 


nrijdJKi.  /m^ 


Michael  A.  Morgan,  (^airman 


Department  of  Electrical  and  Computer  Engineering 


111 


ABSTRACT 


An  algorithm  designed  to  navigate  an  Autonomous  Underwater  Vehicle  (AUV) 
within  a  charted  environment  is  presented.  The  algorithm  processes  sensor  inputs 
from  the  AUV  high  resolution  scanning  sonar,  compass  and  velocimeter.  The 
operating  environment  is  modeled  with  a  suitable  three-dimensional  potential 
function  and  its  gradient  which  form  an  attractive  field.  This  algorithm  provides 
performance  comparable  to  the  Kalman  Filter  with  the  advantage  of  reduced 
computational  requirements  facilitated  by  pre-computation  and  table  look-up  of 
correction  factors.  Applications  of  data  smoothing  filters  and  sliding  mode  theory  are 
also  investigated.  The  appUcability  and  robustness  of  this  approach  are  demonstrated 
with  actual  test  data  obtained  with  the  NFS  Phoenix  submersible  and  extended 


simulation  of  complex  environments  including  unmodeled  obstacles. 


V 


TABLE  OF  CONTENTS 


I.  INTRODUCTION  . 1 

A.  GENERAL  . 1 

B.  EMPHASIS  OF  RESEARCH  . 1 

C.  METHOD  OF  APPROACH . 2 

II.  BACKGROUND . 5 

A.  AUV  MOTION  MODEL . 5 

B.  DESCRIPTION  OF  THE  ENVIRONMENT . 6 

C.  INTERACTION  BETWEEN  MODELS  . 8 

D.  CONVERGENCE  . 10 

E.  IMPORTANCE  OF  PARAMETER  SELECTION . 14 

III.  APPLICATION  OF  POTENTIAL  FUNCTIONS  . 17 

A.  ENVIRONMENTS  WITH  BORDERS  . 17 

B.  SIMULATION  RESULTS . 17 

C.  SIMULATIONS  WITH  ACTUAL  DATA . 19 

D.  INCORPORATION  OF  OBSTACLES  IN  THE  OPERATING 

ENVIRONMENT  . 21 

1.  Unmodeled  Obstacles . 21 

2.  Obstacles  Modeled  in  Potential  Function  . 24 

IV.  APPLICATIONS  IN  COMPLEX  ENVIRONMENTS . 27 

A.  ENVIRONMENTS  WITHOUT  BORDERS  . 27 

B.  SIMULATION  RESULTS . 27 

1.  Accounting  for  Effects  of  Currents . 27 

C.  COMPLEX  ENVIRONMENTS . 31 

D.  COMPLEX  SIMULATION  RESULTS . 33 

vii 


V.  APPLICATION  OF  SLIDING  MODE  THEORY . 37 

A.  SLIDING  OBSERVERS  FOR  NONLINEAR  SYSTEMS . 37 

1.  One-Dimensional  Example . 37 

2.  Verification  of  Convergence . 39 

VI.  SUMMARY,  CONCLUSIONS  AND  RECOMMENDATIONS  . 43 

A.  SUMMARY . 43 

B.  CONCLUSIONS . 43 

C.  RECOMMENDATIONS . 44 

APPENDIX  A.  PARAMETER  ESTIMATION . 45 

APPENDIX  B.  SONAR  CHARACTERISTICS . 51 

APPENDIX  C.  MATLAB  AND  SIMULINK  FILES . 53 

A.  GENERAL  . 53 

B.  PROGRAM  LISTINGS  FOR  SIMULATIONS  IN  CHAPTER  II  ....  53 

C.  PROGRAM  LISTINGS  FOR  SIMULATIONS  IN  CHAPTER  IB  ....  57 

D.  PROGRAM  LISTINGS  FOR  SIMULATIONS  IN  CHAPTER  IV  ...  .  71 

E.  PROGRAM  LISTINGS  FOR  SIMULATIONS  IN  CHAPTER  V  ....  79 

F.  AUV  SIMULINK  MODEL . 84 

APPENDIX  D.  SMOOTHING  FILTER  DESIGN . 87 

LIST  OF  REFERENCES  . 91 


viii 


INITIAL  DISTRIBUTION  LIST 


93 


ACKNOWLEDGEMENTS 


This  thesis  is  dedicated  to  the  memory  of  my  father,  Joseph  H.  Conowitch  (1936- 
1993)  whose  guidance  and  encouragement  continue  to  inspire  me  to  make  him  proud. 

I  would  also  like  to  acknowledge  my  family  Charmaine,  Celeste  and  Roxanne  for 
their  unending  love  and  support  in  all  my  endeavors. 

Lastly,  my  thanks  go  to  Roberto  Cristi,  and  Robert  (Gary)  Hutchins,  two  professors 
who  have  made  my  time  at  the  Naval  Postgraduate  School  an  enjoyable,  challenging,  but 
most  of  all  a  rewarding  experience. 


1.  INTRODUCTION 


A.  GENERAL 

Navigation  is  the  process  of  safely  directing  the  movements  of  a  vehicle  from  one 
point  to  another.  Localization  is  the  process  of  accurately  determining  the  vehicle's  position 
in  space  at  any  given  moment  in  time.  The  investigation  and  development  of  methods  to 
accomplish  both  of  these  related  processes  is  the  topic  of  this  thesis. 

Autonomous  Underwater  Vehicles  (AUV)  are  a  subclass  of  Unmanned  Underwater 
Vehicles  (UUV)  which  operate  under  the  control  of  systems  which  are  an  integral  part  of 
the  vehicle  itself  This  is  in  direct  contrast  to  tethered  and  remotely  operated  vehicles  which 
require  an  interface  and  a  communications  link  with  an  operator  stationed  in  a  separate 
location. 

The  advantages  gained  by  autonomy  from  an  external  control  system  are  obtained 
at  the  cost  of  a  more  complex  on-board  navigation  system.  Highly  accurate  and  reliable 
inertial  navigation  systems  are  readily  available  and  ideally  suited  for  underwater  navigation, 
but  the  size,  weight  and  associated  data  processing  characteristics  of  these  systems  preclude 
their  practical  use  on  small  vehicles  in  the  class  of  AUVs  about  which  we  are  concerned. 

AUVs  have  found  increasing  use  in  areas  such  as  cable-laying,  minefield 
surveillance,  petroleum  drilling,  under-ice  surveillance,  and  underwater  construction  and 
inspection  [Ref  1].  An  AUV  can  be  particularly  useful  in  an  environment  which  is 
hazardous  or  difficult  to  operate  in  for  extended  periods  of  time  for  human  beings. 

B.  EMPHASIS  OF  RESEARCH 

This  thesis  is  concerned  with  the  navigation  of  a  small  class  of  AUV  such  as  the 
NPS  Phoenix.  A  diagram  of  this  vehicle  is  shown  in  Figure  1.1.  The  methods  developed 
will  utilize  the  information  provided  by  installed  sensors  which  include  a  compass  for 
determining  vehicle  heading,  a  velocimeter  or  log  to  measure  vehicle  speed  relative  to  the 
water,  a  fathometer  for  providing  depth-sensing  information,  and  a  high-frequency  scanning 
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Figure  1.1  NFS  Phoenix  AUV 


sonar  providing  range  and  bearing  measurements  to  objects  in  the  vehicle's  operating 
environment.  This  is  the  minimal  set  of  required  instrumentation  to  facilitate  navigation  of 
an  underwater  vehicle  in  three  dimensions.  It  is  the  aim  of  this  thesis  to  develop  an  effective 
algorithm  which  estimates  the  AUV  position  without  utilizing  a  Kalman  Filter.  Ease  of 
implementation,  numerical  stability  and  satisfactory  performance  are  driving  factors  behind 
the  design  of  the  algorithm  as  well  as  a  conscious  effort  to  avoid  the  complexity  and  possible 
numerical  instabilities  associated  with  the  Kalman  Filter. 

C.  METHOD  OF  APPROACH 

Solving  the  short-range  navigation  problem  for  the  AUV  requires  an  algorithm  which 
consists  of  two  main  parts.  Firstly,  a  non-linear  state-space  model  for  motion  of  the  AUV 
is  developed  which  provides  the  AUV  position  and  associated  rates  based  on  the  system 
control  inputs.  Secondly,  a  model  of  the  environment  is  constructed  which  exhibits  several 
desirable  properties.  The  function  which  defines  the  environment  allows  the  precomputation 
of  navigation  correction  factors  for  any  region  of  operation  within  the  mapped  environment. 
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The  onboard  sonar  provides  the  necessary  interaction  between  the  AUV  and  the  environment 
models.  The  sonar  range  and  bearing  measurements  are  added  vectorially  to  the  estimated 
position  of  the  AUV  and  the  corresponding  navigation  correction  factor  is  determined  to 
adjust  the  estimated  position  closer  to  the  actual  position  of  the  vehicle.  This  process  is 
conducted  in  an  iterative  fashion  providing  constant  position  correction  consistent  with  the 
availability  of  sonar  data.  This  results  in  a  simple  Least  Mean  Squares  (LMS)  algorithm, 
which  basically  is  a  simplified  Kalman  Filter.  A  block  diagram  of  the  overall  system  is 
presented  in  Figure  1.2. 


Figure  1.2  System  Block  Diagram 


This  thesis  is  organized  in  six  parts.  Chapter  n  provides  the  theory  behind  the  design 
of  both  the  vehicle  and  environmental  models,  as  well  as  the  interaction  between  the 
twoprovided  by  the  sonar  system.  Chapter  III  applies  the  developed  algorithm  to  closed- 
border  environments  such  as  the  test  pool  utilized  for  data  collection  with  the  NFS  Phoenix. 
Chapter  IV  demonstrates  the  applicability  of  the  algorithm  to  more  realistic  scenarios  of 
open  environments  and  complex  environment  models  composed  of  multiple  geometric 
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shapes  (or  primitives).  Chapter  V  investigates  the  application  of  sliding  mode  theory. 
Finally,  Chapter  VI  summarizes  the  results  and  conclusions  of  this  study  and  provides 
several  recommendations  for  follow-on  research. 
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n.  BACKGROUND 


A.  AUV  MOTION  MODEL 

The  dynamics  of  the  vehicle  are  modeled  in  a  state-space  representation.  For  the 
purpose  of  this  analysis,  the  model  will  be  restricted  to  planar  motion  of  the  AUV  in  a 
cartesian  coordinate  system  with  the  origin  as  the  bottom  left  or  southwest  corner  of  the 
operating  environment.  Defining  the  state  of  the  vehicle  in  terms  of  absolute  position  x,y  and 
velocity,  heading  and  yaw  rate  (v,  0,  0)  results  in  a  five-dimensional  state  vector.  This 
leads  to  a  dynamic  model  of  the  form 

X  =  fix.  If,  w)  (2.1) 


with  X  =  [x  y  V  Q  6]^  e  the  state  vector,  u  =  [i/j  e  the  vector  of 
command  inputs  for  motor  rpm  and  rudder  angle,  and  w  disturbances  which  account  for 
modeling  errors  between  Equation  (2.1)  and  the  actual  physical  dynamics  of  the  vehicle. 
The  differential  equations  which  comprise  the  function  /are 


X  =  VCOS0 
/  =  vsin0 

1?  =  -fliv  . 

0  =  -ajO  .  Ajttj 


(2.2) 


where  flj,  and  ft^are  constants  determined  by  the  vehicle  dynamics. 

A  recursive  least  squares  (RLS)  parameter  estimation  applied  to  vehicle  trajectory 
data  obtained  with  the  NPS  Phoenix  AUV  to  estimate  the  values  of  the  dynamic  parameters 
a^,  a^,  Aj,  and  results  in  the  following  non-linear  state-space  model  with  w 
corresponding  to  the  modeling  errors  ; 
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b 

0 

cos  0p 

0 

0 

0 

0 

0 

0 

sin  00 

0 

0 

0 

0 

0 

0 

-508  .4 

0 

0 

X  + 

1.667 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

-1 

0 

1 

(2.3) 


A  summary  of  the  methodology  and  results  of  the  parameter  estimation  are  included 
in  Appendix  A  This  state-space  model  produces  parameters  in  units  of  feet  (ft)  for  position, 
feet/second  (fps)  for  velocity,  degrees  for  heading  (referenced  to  the  x-axis),  and 
degrees/second  (dps)  for  heading  rate.  As  mentioned  previously,  inputs  are  motor 
revolutions/minute  (rpm)  and  rudder  angle  in  degrees  (positive  values  indicate  left  while 
negative  values  correspond  to  right  rudder  commands). 

This  model  can  be  utilized  in  two  ways.  Firstly,  it  is  used  to  generate  simulated 
motion  data  for  verifying  the  validity  of  the  navigation  algorithm.  Secondly,  it  can  be  used 
as  the  basis  of  an  algorithm  which  solves  for  the  required  control  trajectory  which  causes  the 
vehicle  to  conform  to  a  desired  track  or  path  through  the  environment. 

B.  DESCRIPTION  OF  THE  ENVIRONMENT 

The  operating  environment  of  the  AUV  is  modeled  by  a  function  V  which  associates 
a  potential  field  to  any  point  in  the  environment,  with  the  assumption  that  all  acoustic 
reflective  surfaces  are  equipotential.  This  is  analogous  to  the  electromagnetics  model  of 
electric  charges  at  rest  on  a  conducting  surface,  where  the  electric  potential  is  contant 
and  assumed  to  be  zero.  A  potential  function  of  this  type  then  exhibits  the  following 
desirable  properties; 

•  V(x)  =  Oat  any  point  on  the  boundary  or  reflecting  surface  of  the  environment 

•  V (x)  is  continuous,  differentiable  and  uniformly  bounded  at  any  point. 
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•  Given  any  point  x  on  the  reflective  surface  (such  that  V  (jc)  =  0 ),  for  any  small 
perturbation  6  we  can  write  6^VV(x  +  6)  i  0  (with  V  indicating  the 
gradient  operator). 


The  last  property  simply  states  that  the  vector  field  formed  from  the  spatial  gradient 
is  attractive  toward  the  reflective  surface,  and  there  is  no  component  tangent  to  the  surface 
which  would  violate  the  steady-state  assumption  of  the  field. 

To  illustrate  the  basic  concept  of  the  potential  function,  consider  a  pool  of 
rectangular  shape  with  sides  of  length  L  j  and  along  the  x  and  y  axes  respectively  and  the 
origin  as  the  lower  left  comer  of  the  pool.  The  function 

y)  =  -  L^y{y  -  (2.4) 


equals  zero  at  any  point  on  the  pool  border  and  therefore  satisfies  the  first  property. 

However,  this  function  is  clearly  discontinuous  at  the  comers  of  the  pool  and  in 
order  to  meet  the  requirements  of  the  second  property  P(x,  y)  is  combined  with  an 
auxiliary  function  of  the  form 

JL 

F^(z)  .  '  '  *  ‘  X  >  0  (2.5) 

\  ^  e  ^ 

Therefore, 

V{x,  y)  =  F^(P(x,  y))  (2.6) 

which  meets  the  requirements  of  all  three  properties,  returning  a  value  in  the  range  [-1,1] 
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and  is  dififerentiable  at  any  point  within  the  environment.  A  typical  potential  function  for  a 
rectangular  pool  is  presented  as  Figure  2. 1 . 


Figure  2.1  Potential  Function  for  Rectangular  Pool 


Application  of  the  spatial  gradient  operator  to  the  potential  function  produces  the 
attractive  field  of  Figure  2.2  associated  with  this  function.  The  attractive  field  magnitude 
will  form  the  basis  of  the  navigation  correction  factor  which  is  the  core  of  the  algorithm 
discussed  in  detail  in  the  following  sections  and  implemented  in  scenarios  of  increasing 
complexity. 

C.  INTERACTION  BETWEEN  MODELS 

The  sonar  returns  produced  by  the  AUV  installed  sonar  system  provide  the  required 
interaction  between  the  AUV  dynamic  model  and  the  potential  function  model  of  the 
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environment.  A  summary  of  the  operating  characteristics  of  the  high  frequency  scanning 
sonar  installed  on  the  NPS  Phoenix  AUV  is  included  as  Appendix  B. 


•2  0  2  4  6  8  10  12  14 

x-coordmate  (ft) 


Figure  2.2  Contours  and  Vector  Field  of  Potential  Function 


To  illustrate  the  approach  taken,  consider  a  vector  which  defines  the  position  of  the 
vehicle  at  any  given  time 


(2.7) 


and  define  a  measurement  vector  consisting  of  the  sonar  information 

where  p  represents  the  range  and  a  the  angle  of  the  sonar  return  relative  to  the  x-axis  .  The 
vector  sum  p  +  Ts  defines  the  position  of  the  tip  of  the  sonar  vector  in  cartesian 
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coordinates  with  the  matrix  T  representing  the  transformation  between  vehicle  and  inertial 
coordinate  frames. 

Now  define  a  second  measurement  vector  consisting  of  the  data  provided  by  the 
sensors  monitoring  velocity  and  heading  of  the  vehicle 


2(0 


v(0 

0(0 


(2.9) 


This  results  in  a  model  which  represents  the  dynamics  and  kinematics  of  the  vehicle 
operating  in  the  defined  environment  as  a  system  of  equations  in  the  form 

p  -  m 

i  2  =  f^Z,  u)  (2.10) 

0  -  V(p  *  T(z)s) 


where  the  last  equation  is  obtained  by  recalling  that  the  value  of  the  potential  function  is  zero 
if  the  tip  of  the  sonar  vector  falls  on  the  boundary  (sonar  reflecting  surface)  of  the 
environment. 


D.  CONVERGENCE 

If  we  define  the  vehicle's  estimated  position  as  p  then  it  can  be  updated  as 

p  =/i(2j  *  \i'7V(p  ^  Ts)  (2.11) 


where  p  is  a  positive  constant  defined  as  the  navigation  correction  factor  gain  (or  step  size). 

4 

Clearly,  if/  =  p  (no  error  in  position  estimation),  tiie  second  tarn  above  will  be  zoo  and/  =  /i(zj 
as  expected  (neglecting  noise). 

For  the  situation  where  an  acceleration  measurement  is  available,  as  would  be  the 
case  with  an  inertial  navigation  system  which  includes  accelerometers,  the  correction  can 
be  applied  to  the  position  and  velocity  estimate  and  can  be  updated  as 
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< 


/  =  V  +  \ij^V(p  +  Ts) 
^  =  a  *  *  Ts) 


(2.12) 


with  Hj,  Hj  being  positive  constants. 

To  demonstrate  local  convergence  of  this  method  (for  the  general  case  where  a 
velocity  measurment  is  available)  define  the  position  error  as  /  =  /  -  p.  Combining 
equations  (2. 10)  and  (2. 1 1)  yields  the  error  equation 

p  ^  yi^Vip  *  Ts)  (2.13) 

and  pre-multiplying  by  the  transpose  of  the  error  yields 

P^P  “  \ip^^V(p  *  Ts  *  p)  (2.14) 

where  we  recognize  that  the  point  p  +  Ts  is  on  the  reflective  surface  and  that  for  p 
sufficiently  small  that  the  right  hand  side  of  the  above  equation  is  non-positive  by  the  third 
property  specified  for  the  potential  function  and  |p|^is  decreasing  with  time.  Integration 
of  equation  (2. 14)  with  respect  to  time  yields 

-d/wi’  -  i/(o)i")  ■ 

2 

f  (2.15) 

HJp(t)^VF(p  +  Ts  *  p(x))d'v 
0 

and  since  the  integrand  is  nonpositive  for  all  t  it  will  go  to  zero  as  time  tends  to  infinity. 
This  impUes  that  the  error  vector  p  tends  to  be  orthogonal  to  the  gradient  of  the  field,  and 
that  the  point  p  *  Ts  tends  to  be  on  the  reflecting  surface.  The  mathematical  verification 
for  the  proof  of  convergence  in  the  special  case  of  acceleration  vice  velocity  measurements 
is  addressed  in  [Ref  2].  For  the  majority  of  simulations  addressed  in  this  thesis,  velocity 
measurements  will  be  assumed  available  and  utilized  for  position  estimation.  This 
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assumption  is  critical  to  the  accurate  navigation  algorithm  performance  in  data-sparse 
environments  to  be  addressed  in  Chapter  IV. 

As  a  graphical  demonstration  of  convergence,  consider  the  following  simple 
scenario.  An  AUV  is  moving  in  a  horizontal  path  with  constant  velocity  towards  a  reflective 
boundary  (wall).  The  initial  conditions  are; 

[  x(0)  =  3  / 

^  v(0)  =  0.2  fils 

x(0)  =  4  ft 

Application  of  the  position  estimation  algorithm  (velocity  measured)  with  an 
integration  step  size  of  0.0225  seconds  (also  the  sonar  ping  interval)  yields  the  estimated 
position  trajectory  of  Figure  2.3.  The  position  estimate  converges  rapidly  to  the  actual 
position.  Repeating  the  simulation  of  the  same  scenario  but  with  acceleration  vice  velocity 
measurements  results  in  the  trajectories  for  estimated  position  and  velocity  of  Figure  2.4. 
Note  that  the  position  estimate  converges  in  approximately  the  same  amount  of  time  as  for 
the  previous  case,  while  the  velocity  takes  slightly  longer  to  converge  and  exhibits  greater 
statistical  variation  as  can  be  anticipated  from  theory. 
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Figure  2.4  Verification  of  Convergence  with  Measured  Acceleration 
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E.  IMPORTANCE  OF  PARAMETER  SELECTION 

The  proper  selection  of  the  parameter  A,  used  in  Equation  (2.5)  to  describe  the 
auxiliary  function  is  of  vital  importance  in  that  it  affects  the  width  of  the  attractive  region 
around  the  boundary  or  reflective  surface  of  the  potential  function  approximating  the 
environment.  If  the  tip  of  the  sonar  vector  does  not  fall  within  the  attractive  region  then 
negligible  correction  to  the  estimated  position  of  the  vehicle  will  occur.  However,  if  the 
attractive  region  is  too  wide  then  it  may  include  objects  or  obstacles  which  are  not  modeled 
in  the  environment.  A  sonar  return  off  such  an  obstacle  could  be  construed  as  a  return  off 
the  reflective  surface  generating  the  field,  thereby  producing  an  erroneous  correction  factor. 

In  previous  navigation  algorithms  designed  for  implementation  on  the  NFS  AUV 
[Refs.  3  and  4]  the  non-linear  potential  function  of  sonar  information  was  utilized  as  the 
basis  for  correction  factors  in  an  Extended  Kalman  Filter  (EKF)  where  the  magnitude  of 
F(x)  had  a  lesser  effect  on  algorithm  performance  than  for  our  application.  For  the  current 
design  the  magnitude  as  well  as  the  width  of  the  attractive  field  is  crucial  to  the  choice  of 
a  constant  gain  correction  factor  defining  an  appropriate  error  correction  over  a  wide  range 
of  position  errors.  In  addition,  as  the  complexity  of  the  environment  increases  the  values  of 
X  and  p  in  Equations  (2.5)  and  (2..1 1)  must  be  carefully  selected  due  to  the  multiplicative 
effect  of  potential  functions  based  on  multiple  objects,  as  will  be  discussed  in  Chapter  IV. 

Figure  2.5  shows  the  effect  of  varying  X  on  the  magnitude  and  width  of  the  attractive 
region  formed  by  the  rectangular  environment  potential  function  at  the  reflective  boundary. 
The  factor  p  represents  the  size  of  the  correction  (usually  called  the  step  size)  and  it  affects 
both  the  convergence  rate  and  the  stability  of  the  algorithm. 
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Figure  2.5  Effect  of  Varying  Lamda  on  the  Navigation  Correction  Factor 


In  the  next  chapter  these  theoretical  concepts  will  be  implemented  in  an  algorithm 
which  utilizes  the  potential  function  approach  to  solve  the  two-dimensional  short  range 
navigation  problem  of  an  AUV. 
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m.  APPLICATION  OF  POTENTIAL  FUNCTIONS 


A.  ENVIRONMENTS  WITH  BORDERS 

In  this  chapter  we  address  the  problem  of  applying  the  potential  function  to  a 
structured  environment.  Although  we  will  be  using  the  model  of  the  test  tank  at  the  Naval 
Postgraduate  School  Annex,  the  results  can  be  generalized  to  more  complex  environments, 
such  as  pipelines,  oil  rigs,  harbors,  etc.  The  rectangular  environment  with  closed  borders  is 
the  simplest  realistic  operating  area  which  can  be  modeled  with  the  potential  function 
approach  and  would  reflect  operations  such  as  acoustic  test  data-gathering  with  the  NFS 
Phoenix  AUV  in  the  laboratory  pool. 

B.  SIMULATION  RESULTS 

The  navigation  algorithm  is  tested  on  a  rectangular  operating  environment  of  size  12 
feet  by  12  feet.  The  motion  of  the  AUV  is  simulated  with  a  non-linear  state-space  model 
with  sensor  noise  applied  to  the  velocity  and  heading  measurements.  The  sonar  system 
operation  is  also  simulated  with  noise  applied  to  both  range  and  bearing  measurements.  All 
sensor  noise  is  modeled  with  a  gaussian  zero-mean  distribution.  The  SBVIULINK  models  and 
MATLAB  code  utilized  for  the  simulation  are  included  as  Appendix  C.  Figure  3.1  shows 
the  results  of  a  72  second  simulation  which  encompasses  eight  360-degree  sonar  scans  of  the 
environment  border  and  graphically  compares  the  actual  and  estimated  trajectories  of  the 
vehicle  as  well  as  plotting  the  estimated  borders  of  the  pool. 

As  can  be  seen  from  the  plot,  the  algorithm  quickly  drives  the  estimated  trajectory 
to  the  actual  track  of  the  vehicle.  Of  interest  is  the  "staircase-like"  appearance  of  the 
estimated  trajectory.  This  is  a  result  of  the  fact  that  when  sonar  returns  are  falling  on  the 
vertical  borders  of  the  pool  only  corrections  to  the  x-dimension  of  velocity,  and  hence 
position,  can  be  obtained.  Similarly,  sonar  returns  off  of  the  horizontal  boundaries  correct 
only  the  y-dimension  components  of  position  and  velocity.  This  effect  is  minimized  by 
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Figure  3.1  Estimated  vs.  Actual  Vehicle  Trajectory 

utilizing  a  smoothing  filter  to  process  the  estimated  position  results.  The  effects  of  applying 
the  algorithm  are  presented  in  Figure  3.2  which  compares  the  actual  track  to  the  smoothed 
estimate  for  the  same  scenario  utilized  for  the  simulation  in  Figure  3.1. 
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Figure  3.2  Effect  of  Smoothing  Position  Estimates 

Smoothing  of  the  estimated  position  data  results  in  an  estimate  that  follows  the  actual 
trajectory  with  a  high  degree  of  accuracy.  The  disadvantage  of  data  smoothing  is  that  it 
generates  a  time  delay  in  producing  a  position  estimate,  since  the  filter  utilizes  the  previous 
400  data  points  (one  sonar  scan  worth  of  data)  for  weighted  averages.  The  details  of  the  filter 
design  are  included  as  Appendix  D. 

C.  SIMULATIONS  WITH  ACTUAL  DATA 

In  order  to  demonstrate  the  robustness  of  the  navigation  algorithm,  simulations  with 
actual  sonar  data  gathered  in  the  test  pool  were  conducted.  In  addition,  velocity  and  heading 
sensor  information  is  not  used  in  forming  the  position  estimate,  which  is  a  more  stringent 
requirement  than  the  standard  initial  simulation  assumptions  of  available  data.  Figure  3.3 
shows  a  typical  data  run  for  the  AUV  operating  in  a  six  meter  by  six  meter  test  pool. 

The  available  sonar  data  is  excessively  corrupted  by  noise  as  a  result  of  reverberation 
caused  by  confining  the  sonar  acoustic  energy  in  a  relatively  small  water  volume.  This  effect 
is  evident  through  the  multiple  false  returns  both  inside  and  outside  of  the  pool  boundaries. 
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Figure  3.3  Actual  Sonar  Test  Data 


Interpretation  of  the  sonar  data  is  further  complicated  by  an  obstacle  placed  in  the  lower 
right-hand  quadrant  which  is  not  modeled  in  the  potential  function. 

When  vehicle  velocity  and  heading  information  are  available,  the  navigation 
correction  factor  adjusts  the  sensed  velocity  cartesian  components  to  drive  the  distance 
between  the  estimated  position  and  environment  boundary  to  match  the  sonar  range.  Without 
velocity  information  a  higher  navigation  correction  factor  gain  is  required  to  compensate  for 
the  lack  of  a  velocity  signal  driving  the  estimated  position  of  the  vehicle.  Application  of  the 
navigation  algorithm  to  the  sonar  data  of  Figure  3.3  results  in  the  estimated  environment  and 
vehicle  trajectory  of  Figure  3.4. 

Although  there  are  still  a  number  of  false  sonar  returns  and  gaps  in  the  estimated 
border  of  the  environment,  the  algorithm  is  clearly  able  to  ascertain  the  track  of  the  vehicle 
and  the  correct  size  and  orientation  of  the  test  pool  without  utilizing  velocity  and  heading 
data  input. 
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environment's  potential  function.  For  this  reason  it  is  essential  that  parameter  A,  in  the 
potential  function  is  chosen  so  that  the  attractive  region  is  wide  enough  to  provide  adequate 
correction,  but  narrow  enough  to  be  robust  in  the  presence  of  unmodeled  obstacles.  Figure 
3.5  illustrates  a  pool  simulation  with  unmodeled  obstacles  at  coordinates  (3,3)  and  (9,9).  The 
effects  of  applying  the  smoothing  filter  to  the  estimated  track  of  the  vehicle  are  presented 
as  Figure  3.6. 


Figure  3.5  Simulation  with  Unmodeled  Obstacles 


In  order  to  ascertain  the  effect  of  an  obstacle  which  falls  in  the  attractive  region  of 
the  potential  function,  the  simulation  was  re-run  with  the  lower  left-hand  obstacle  moved 
to  coordinates  (1,3)  which  is  in  the  attractive  field  of  the  left  pool  edge.  Figure  3.7  shows 
the  results  of  thi.s  simulation.  Note  that  the  erroneous  correction  results  in  greater  statistical 
fluctuation  in  the  x-dimension  yet  the  algorithm  is  still  capable  of  producing  convergence 
of  the  position  estimate  with  the  actual  trajectory.  The  effect  of  applying  the  smoothing  filter 
is  seen  in  Figure  3.8  where  virtually  no  difference  is  distinguishable  from  the  less  complex 
scenario  of  Figure  3.6. 
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Unmodeled  Obstacles  with  Smoothing  Filter 


Figure  3.6  Unmodeled  Obstacles  with  Smoothing  Applied 


Unmodeled  Obstacle  in  Attractive  Fieid 


x-coordinate  (ft) 


Figure  3.7  Simulation  with  Unmodeled  Obstacle  in  Attractive  Field 
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Figure  3.8  Unmodeled  Obstacle  in  Attractive  Field  with  Smoothing  Applied 


2.  Obstacles  Modeled  in  Potential  Function 

Objects  in  the  operating  environment  which  are  in  known  locations  can  be  utilized 
to  provide  geographic  references  to  the  navigation  algorithm  in  the  same  way  that  the  pool 
boundaries  provide  a  frame  of  reference  for  the  test  pool  case.  These  objects  can  either  be 
those  already  in  the  operating  environment  such  as  geographic  features,  pier  pilings,  quay 
walls,  etc.  or  those  intentionally  place  in  the  environment  by  the  AUV  operators  such  as 
marker  bouys  or  sonar  reflectors.  This  section  is  primarily  concerned  with  the  latter  to 
demonstrate  the  ability  to  use  an  object  place  in  the  operating  area  at  a  known  location  to 
provide  geographic  reference  to  the  navigation  algorithm  in  a  data-poor  operating 
environment.  As  shown  in  Chapter  11,  incorporating  additional  geometric  features  in  the 
potential  function  is  relatively  simple.  The  obstacles  will  be  modeled  as  a  six-inch  diameter 
cylinder  located  in  the  horizontal  operating  plane  of  the  vehicle.  The  details  of  the  potential 
function  calculation  will  be  deferred  to  the  next  chapter  where  potential  function 
descriptions  for  a  number  of  geometric  primitives  will  be  addressed.  Figure  3.9  demonstrates 
the  effect  of  including  an  obstacle  (marker)  in  the  potential  function.  The  attractive  "well” 
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of  the  marker  at  coordinates  (3,3)  is  clearly  seen  in  the  3-dimensional  plot  of  the  potential 
function. 

Figures  3.10  and  3.11  illustrate  a  simulation  with  markers  (obstacles)  at  the  same 
location  as  in  the  simulation  of  Figure  3.5  and  the  marker  at  coordinates  (3,3)  modeled  in 
the  potential  function.  The  values  of  A.  and  p  are  chosen  such  the  the  marker  provides  the 
majority  of  the  navigation  correction.  By  comparing  this  to  the  aforementioned  simulation 
it  can  be  seen  that  the  algorithm  converges  more  slowly  until  the  sonar  beam  falls  on  the 
marker,  at  which  time  the  estimated  postition  rapidly  converges  to  the  actual  vehicle 
position.  In  addition,  there  is  less  statistical  variation  in  the  estimated  trajectory  since  larger 
corrections  occur  only  over  a  small  portion  of  the  sonar  scan  (when  the  obstacle  falls  within 
the  acoustic  beam  of  the  sonar). 


Figure  3.9  Potential  Function  with  Modeled  Obstacle 


In  the  next  chapter  the  concepts  introduced  here  are  extended  to  environments  and 
scenarios  of  greater  complexity  in  that  data  required  for  updating  the  vehicle  estimated 
position  will  not  be  continuously  available. 

25 


Figure  3.10  Obstacle  Modeled  in  Potential  Function 


Modeled  Obstacle  with  Smoothing  Fitter 


x-coordinate  (ft) 

Figure  3.11  Modeled  Obstacle  with  Smoothing  Applied 


26 


IV.  APPLICATIONS  IN  COMPLEX  ENVIRONMENTS 


A.  ENVIRONMENTS  WITHOUT  BORDERS 

In  an  environment  without  continuous  borders  the  navigation  correction  factor  is 
applied  only  when  the  sonar  acoustic  beam  falls  on  an  obstacle  in  the  vehicle  operating  area. 
These  obstacles  can  either  be  placed  in  the  environment  as  a  marker  or  pre-existing 
geographic  features.  The  availability  of  heading  and  velocity  sensor  data  is  critical  to  the 
proper  operation  of  the  navigation  algorithm  in  an  open  environment  because  corrections  to 
the  estimated  position  of  the  vehicle  will  occur  only  over  a  relatively  small  portion  of  each 
360-degree  sonar  scan.  In  addition,  currents  and  tidal  effects  in  the  operating  area  can 
generate  additional  position  and  velocity  errors  which  must  be  considered. 

B.  SIMULATION  RESULTS 

The  algorithm  is  applied  to  an  open  environment  which  includes  two  marker  buoys 
to  provide  the  required  navigation  references.  Each  buoy  is  a  six-inch  diameter  cylinder 
placed  in  the  horizontal  operating  plane  of  the  vehicle.  Figure  4.1  shows  the  potential 
function  for  this  scenario  where  the  attractive  wells  of  the  two  obstacles  at  coordinates  (5,5) 
and  (20,20)  are  clearly  seen.  Figure  4.2  illustrates  the  contours  and  associated  attractive 
vector  field  of  this  function.  The  results  of  a  90  second  simulation  consisting  of  ten  360- 
degree  sonar  scans  are  presented  as  Figure  4.3.  Application  of  the  smoothing  filter  is 
unnecessary  in  this  case  since  the  staircase-like  appearance  of  the  estimated  trajectory  is  not 
evident  without  continuous  navigation  corrections. 

1.  Accounting  for  Effects  of  Currents 

Currents  in  the  vehicle  operating  area  require  a  modification  to  the  state-space  model 
utilized  to  generate  the  actual  trajectory  of  the  AUV.  The  current  is  assumed  to  be  slowly 
varying  with  time  and  can  be  measured  or  estimated  prior  to  actually  operating  the  vehicle 
in  the  environment.  Adding  two  components  to  the  state  vector  to  account  for  the  current 
components  in  the  x  and  y  dimensions  yields  the  modified  state-space  model: 


27 


Figure  4.3  Simulation  in  Environment  without  Borders 
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(4.1) 


Rerunning  the  simulation  of  Figure  4.3  and  utilizing  the  revised  vehicle  model  with 
a  small  current  traversing  the  operating  area  results  in  the  actual  vehicle  trajectory  of  Figure 
4.4.  The  results  of  applying  the  navigation  algorithm  are  presented  as  Figure  4.5  for  this 
scenario. 
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Figure  4.4  Effect  of  Current  on  Vehicle  Track 


Figure  4.5  Simulation  with  Current  in  Operating  Area 
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As  we  can  see  in  these  simulations,  the  current  has  a  significant  effect  on  the 
estimated  trajectory  during  the  portions  of  the  sonar  scan  where  corrections  are  not  available. 
This  is  due  to  the  difference  between  the  vehicle  sensor  velocity  as  measured  relative  to  the 
water  and  the  inertial  velocity  measured  relative  to  the  ground  which  includes  the  effect  of 
the  current.  Each  pass  of  the  sonar  beam  over  an  obstacle  causes  the  estimated  position  to 
"jump"  towards  the  actual  trajectory.  Increasing  the  number  of  markers  or  navigational 
references  counters  this  effect  by  providing  more  frequent  correction  to  the  position 
estimate. 

C.  COMPLEX  ENVIRONMENTS 

The  rendering  of  a  realistic  environment  into  an  effective  mathematical  function 
which  accurately  describes  its  critical  geographic  features  requires  additional  complexity 
over  the  potential  functions  investigated  previously. 

Recall  that  the  describing  function  of  the  environment  must  equal  zero  if  the  tip  of 
the  sonar  vector  falls  on  the  acoustic  reflecting  border  of  the  environment,  i.e. 

P()t,  ^).0  (4.2) 

Consider  an  environment  consisting  of  n  components,  each  with  their  own  describing 
mathematical  function  and  the  overall  function  being  the  product  of  each  component 
individual  function: 


P  (X,  y) 


n 


/-I 


y) 


(4.3) 


Utilizing  this  approach  results  in  a  function  which  will  be  zero  if  the  tip  of  the  sonar 
vector  falls  on  any  component  of  the  overall  environment  model.  Application  of  the 
auxiliary  function  of  Equation  (2.5)  results  in  a  potential  function  meeting  the  three 
requirements  set  forth  in  Chapter  II  for  the  environment  describing  function. 
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Therefore, 


ft 

V(x,  y)  ■  y)) 


The  five  model  components,  or  geometric  primitives,  chosen  to  mathematically 
describe  the  environment  are  summarized  in  Figure  4.6. 


Figure  4.6  Environment  Model  Components 


A  wide  variety  of  operating  environments  and  geographic  features  can  be  modeled 
utilizing  these  components.  For  example  a  harbor  scenario  could  be  modeled  which 
incorporates  piers,  quay  walls,  moored  vessels,  fixed  navigation  aids,  bottom  contours,  etc. 
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D.  COMPLEX  SIMULATION  RESULTS 

A  scenario  similar  to  the  one  discussed  in  the  previous  section  was  simulated  with 
the  AUV  operating  in  a  modeled  harbor.  The  environment  consists  of  a  vertical  segment,  a 
horizontal  segment,  an  angled  segment  and  one  marker  buoy.  This  operating  area  is  open  on 
two  sides.  Currents  are  considered  to  be  neghgible  as  might  be  the  case  in  a  sheltered  harbor. 
Figures  4.7  and  4.8  show  the  potential  function  and  its  associated  contours  for  this  scenario. 
The  results  of  applying  the  navigation  algorithm  are  presented  as  Figure  4.9. 


Because  the  sonar  pointing  angle  is  initialized  at  zero  degrees,  approximately  one- 
half  of  a  sonar  scan  passes  before  any  correction  to  the  estimated  position  occurs.  As  soon 
as  the  sonar  returns  off  modeled  environment  components  are  obtained,  the  estimated 
trajectory  rapidly  converges  to  the  actual  vehicle  track. 
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Contours  of  Complex  Environment  Potential  Function 
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Figure  4.8  Contours  of  Potential  Function 
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Figure  4,9  Complex  Environment  Simulation 
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In  this  chapter  we  have  extended  the  algorithm  designed  with  the  potential  function 
approach  to  apply  to  a  number  of  general  scenarios  of  increasing  complexity.  The  robustness 
of  this  approach  was  verified  through  computer  simulation  of  these  scenarios.  In  the  next 
chapter  an  alternative  estimation  strategy  will  also  be  investigated. 
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V.  APPLICATION  OF  SLIDING  MODE  THEORY 


A.  SLIDING  OBSERVERS  FOR  NONLINEAR  SYSTEMS 

As  previously  mentioned,  the  estimator  developed  in  this  research  is  not  based  on  the 
Kalman  Filter,  unlike  most  available  approaches.  Simplicity  of  implementation,  proven 
robustness  and  better  numerical  performance  are  the  basis  of  this  choice.  In  this  chapter  we 
present  a  nonlinear  estimator  (observer)  based  on  sliding  mode  theory,  widely  applied  in 
robust  control  systems,  but  still  not  widely  used  in  state  estimation.  Initial  research  in  this 
area  shows  that  sliding  observers  have  promising  properties  in  the  presence  of  modeling 
errors  and  sensor  noise  [Ref  5].  The  one-dimensional  case  will  be  considered  here  and  then 
extended  to  two-dimensions  with  a  closed-border  simulation  presented  as  an  example 
application  of  the  sliding  mode  observer. 

1.  One-Dimensional  Example 

Consider  a  simple  case  of  a  vehicle  moving  along  the  x-axis  towards  a  reflective 
surface  from  which  sonar  returns  are  obtained.  This  scenario  is  presented  graphically  in 
Figure  5.1.  In  lieu  of  the  potential  function  previously  described,  the  boundary  is  described 
mathematically  by  the  function 

V{x)  =  \L^  -  x\  (5.1) 


where  Z  j  is  the  x-coordinate  of  the  reflective  surface. 
The  motion  model  can  now  be  represented  as 


(5.2) 


where  the  states  Xjand  Xj  are  position  and  velocity  respectively,  and  a^is  acceleration  in 
the  x-dimension. 
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In  this  particular  case,  the  equations  of  the  observer  are 


^1  =  Xj  -  k^VVix^  t  p) 

4  =  *  P) 


(5.3) 


Figure  5.1  Sliding  Mode  Model  in  One  Dimension 

where  and  k^  are  chosen  to  meet  the  sliding  mode  criteria  as  will  be  discussed  shortly. 


We  now  define  the  state  error  as 


(5.4) 


and  taking  the  time  derivative  of  Equation  (5.4)  yields  the  error  rate  equations 
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(5.5) 


which  combined  with  Equations  (5.3)  and  (5.5)  yields  the  following  result 
K  =  ^  ^1  ^  P) 


Recalling  that  the  gradient  of  r(x)  =  0  at  x  =  Z  j  and  ±  1  at  any  other  value  of  x, 
Equation  (5.6)  can  be  rewritten  as 


i  1  =  Xj  -  sign  (Xj) 

4  =  -^2  (^i) 


(5.7) 


which  defines  the  first-order  observer  for  the  x-dimension  with  a  single  measurement  of 
sonar  range  p. 

The  values  of  and  must  be  selected  such  that  the  phase-plane  trajectories 
exhibit  sliding  behavior.  The  required  condition  [Ref.  5]  is  specified  as 

I  X2(0)  I  s  Xj  =  0  (5.8) 

which  determines  the  value  of  k^.  The  value  of  k^  is  chosen  for  a  desired  rate  of 
convergence  of  the  state  estimate. 

2.  Verification  of  Convergence 

In  order  to  verify  satisfactory  performance  of  the  theoretical  sliding  observer  the 
above  algorithm  was  implemented  for  the  same  one-dimensional  scenario  addressed  in 
Chapter  II  to  verify  convergence  of  the  LMS  algorithm.  The  results  of  the  simulation  are 
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included  as  Figure  5.2  where  it  can  be  seen  that  the  observer  rapidly  drives  the  position 
estimate  trajectory  to  the  actual  trajectory  of  the  vehicle. 


Figure  5.2  Verification  of  Convergence  with  Sliding  Observer 


The  sliding  mode  observer  theory  was  extended  to  two  dimensions  and  utilized  to 
simulate  a  closed  border  scenario  similar  to  those  of  Chapter  2.  The  MATLAB  code  for  this 
algorithm  is  included  in  Appendix  C.  Figure  5.3  shows  the  results  of  this  simulation  for  a 
72  second  run  consisting  of  eight  360-degree  sonar  scans  of  the  environment. 

The  algorithm  calculates  and  applies  correction  factors  to  the  x-dimension  position 
and  velocity  only  when  sonar  returns  are  off  of  the  vertical  walls  and  y-dimension 
corrections  are  obtained  only  when  the  sonar  beam  falls  on  the  horizontal  walls.  The 
alternate  coordinate  position  and  velocity  are  updated  utilizing  dead-reckoning  of  position 
with  the  sensed  velocity  component  during  the  passage  of  the  sonar  beam  over  each  side. 
This  also  causes  the  staircase-like  appearance  noted  with  the  previous  algorithm.  Figure  5.4 
shows  the  results  after  application  of  the  smoothing  filter. 
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The  associated  position  and  velocity  trajectories  are  presented  in  Figure  5.5  to 
graphically  illustrate  convergence  of  this  method  in  a  two-dimensional  application. 


time  (sec)  time  (sec) 

Figure  5.5  Sliding  Mode  Observer  Simulation  Trajectories 

Comparison  of  these  results  to  the  similar  scenario  of  Figures  3 . 1  and  3 .2  shows  that 
this  method  produces  comparable  results  for  estimating  the  vehicle  position. 

This  chapter  presented  the  initial  results  of  an  investigation  into  a  second  alternative 
to  the  Kalman  Filter  for  vehicle  state  estimation.  The  viability  of  this  algorithm  was 
demonstrated  for  a  simple  one-dimensional  scenario  and  extended  to  the  two-dimensional 
closed  border  environment  discussed  in  Chapter  II. 
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VI.  SUMMARY,  CONCLUSIONS  AND  RECOMMENDATIONS 


A.  SUMMARY 

This  study  investigated  the  problem  of  navigating  and  localizing  the  position  of 
an  autonomous  underwater  vehicle  in  a  partially  known  environment.  A  navigation 
algorithm  was  designed  based  on  a  potential  function  description  of  the  operating 
environment  combined  with  a  Least  Mean  Squares  (LMS)  estimator.  The  approach  taken 
in  the  design  and  evaluation  of  the  navigation  algorithm  included: 

•  The  development  of  a  state-space  model  describing  the  full  state  of  AUV 
motion. 

•  The  development  of  a  potential  function  description  of  the  operating 
environment. 

•  Testing  of  the  algorithm  in  scenarios  of  increasing  complexity. 

•  Investigation  of  an  alternative  method  utilizing  a  sliding  mode  observer  as  a 
performance  comparison. 

B.  CONCLUSIONS 

The  following  conclusions  have  been  reached  as  a  result  of  this  study: 

•  Estimation  of  vehicle  position  and  velocity  is  possible  with  the  algorithm 
presented. 

•  The  potential  function  can  adequately  describe  complex  operating  scenarios 
and  environments. 

•  The  algorithm  performed  adequately  through  a  variety  of  scenarios  including 
tests  with  actutd  sonar  data  gathered  by  an  AUV. 

•  The  algorithm  is  directly  applicable  to  the  NFS  Phoenix  AUV. 
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C.  RECOMMENDATIONS 


Having  demonstrated  the  reliability  of  the  approach  taken  in  this  study,  the 
following  recommendations  are  made  for  follow-on  research  in  the  on-going  AUV 
development  effort  at  the  Naval  Postgraduate  School: 

•  Convert  the  algorithm  program  to  the  C  programming  language  for  direct 
implementation  into  the  NPS  Phoenix  control  system. 

•  Generate  sonar  test  data  for  scenarios  of  greater  complexity  to  allow  testing  of 
the  algorithm  which  goes  beyond  simulation/modeling. 

•  Develop  an  adaptive  control  strategy  combining  a  sliding  mode  controller  with 
the  environment  potential  function  to  incorporate  obstacle  avoidance  and  real¬ 
time  updating  of  the  environment  map. 
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APPENDIX  A.  PARAMETER  ESTIMATION 


The  subject  of  parameter  estimation  is  addressed  extensively  in  the  technical 
literature.  The  methods  utilized  for  this  thesis  follow  the  derivations  and  procedures 
provided  in  [Refs.  6  and  7], 

The  state-space  model  for  the  AUV  addressed  in  Chapter  2  requires  estimation  of  the 
four  parameters  which  define  the  velocity  and  heading  state  differential  equations 


tJ  =  -a,v  * 

0  =  -fljO  + 


(A.1) 


where  we  recall  that  t/jand  i/j  represent  the  commanded  inputs  of  motor  rpm  and  rudder 
angle  respectively. 

A  first-order  autoregressive  extended  model  (ARX)  of  the  form 

Aiz'^)yit)  =  B(z'^)u(t)  *  e(t)  (A.2) 


is  used  with  y(t),  u(t)  input  and  output  sequences,  and  e(t)  zero  mean  white  noise.  A  time 
delay  is  represented  by  z  and  A  and  B  are  the  polynomials 

A(z'^)  «  1  +  a.z'^  *  ...  +  az'^ 

5(z->)  =  b,z-^  .  ...  .  b„z-^ 

In  order  to  estimate  the  a^and  ft^parameters  from  data  sequences  (y)  and  (u),  the 
model  of  Equation  (A.  2)  is  written  in  regressive  form  as 
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>.(0  =  ^  eit) 

3  {-yit  -  1),  ...  ,-y{t  -  n),  u{t  -  1),  ...  ,«(/  -  «)]  (A.4) 

e  =  [aj,  ...  a„,  b^,  ...  AJ 

The  recursive  least-squares  (RLS)  equations  can  then  be  written  as 

0(/ .  1)  =  0(0  .  A'COIXO  -  ^^(00(0] 

^•(0  =  P(0<^(0[1  -  ‘^^(0P(0‘^(0]  ‘ 

p{t .  1)  =  p(o  -  p(o<>(o^^(OP(0[i .  $^(OP(o^(or' 

Vehicle  data  obtained  from  the  AUV  trajectory  of  Figure  A.  1  was  utilized  to  estimate 
the  system  parameters  of  the  state-space  model.  The  MATLAB  script  file  PARAMEST.m 
implements  the  RLS  algorithm  and  produced  the  parameter  trajectories  of  Figure  A.2. 


Figure  A.1  AUV  Trajectory  for  Parameter  Estimation 
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Figure  A.2  Discrete  Time  System  Parameters 

Conversion  of  the  steady-state  discrete  time  parameters  results  in  the  following 
values  utilized  for  the  continuous-time  nonlinear  state  space  model  parameters; 


aj  =  508.4 
=  1.667 
«2  =  1 
*2=1 


(A.6) 
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File  PARAMEST.m 


%%%% 

%%%% 

%%%%  This  script  file  estimates  the  state-space  model 

%%%%  parameters  utilizing  a  Recursive  Least-Squares  algorithm 
%%%%  based  on  an  input  data  file  containing  velocity  and  heading 
%%%%  data  for  the  NFS  Phoenix  AUV. 

%%%% 

load  prmsdata  %  Load  data  file 

time=prmsdata( : ,  1 );  %  Time  (sec) 

v=prmsdata(:,2);  %  Velocity  (ft/s) 

hdg=prmsdata(: ,3);  %  Heading  Rate  (deg/s) 

rpm=prmsdata(;,4);  %  Motor  Command  (rpm) 
rud=prmsdata(;,5);  %  Rudder  Angle  (deg) 
n=length(v); 

Ts=0.0225;  %  Specify  Time  Step 

%  Estimate  Velocity  Parameters  with  First  Order  ARX  Model 
thl=zeros(2,n);  %  Initialize  Output  Matrix 

P 1 = 1 000 *  eye(2);  %  Initialize  Error  Covariance 

for  i=2:n-l 

phi  1  (i, ;)=[-v(i- 1)  rpm(i- 1)];  %  Data  Input 

%  Calculate  Gain 

K 1  (;,i)=P  1  *phil(i,  :)'*inv(l+phi  1  (i,  :)*P  1  *phi  l(i, :)'); 

%  Update  Covariance  Matrix 

Pl=Pl-Pl*phil(i,:)'*phil(i,:)*Pl*inv(l+phil(i,:)*Pl*phil(i,:)'); 

%  Parameter  Estimate  Update 
thl(:,i+l)=thl(:,i)+Kl(;,i)*(v(i)-phil(i,:)*thl(:,i)); 
end 
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%  Estimate  Heading  Parameters 
th2=zeros(2,n);  %  Initialize  Output  Matrix 

P2=1000*eye(2);  %  Initialize  Error  Covariance 

for  i=2:n-l 

phi2(i,:)=[-hdg(i-l)  rud(i-l)];  %  Data  Input 
%  Calculate  Gain 

K2(:,i)=P2*phi2(i,:)'*inv(l+phi2(i,:)*P2*plii2(i,;)'); 

%  Update  Covariance  Matrix 

P2=P2-P2*plii2(i,:)'*phi2(i,:)*P2*inv(l+plii2(i,:)*P2*plii2(i,:)'); 
%  Parameter  Estimate  Update 
th2(:a+l)=th2(:,i)+K2(:4)*(hdg(i)-pW2(i,:)*tli2(:,i)^ 
end 

%  Convert  Discrete  Time  Parameters  to  Continuous  Parameters 
pliil=[-thl(l,n)  0;0  -th2(l,n)]; 
dell-[thl(2,n)  0;0  th2(2,n)]; 

[A1  ,B  1  ]=d2c(phi  1 ,  del  1  ,Ts); 
prms=[-Al(l,l);Bl(l,l);-Al(2,2);Bl(2,2)]; 

%  Plot  Discrete  Time  Parameters 
clg 

subplot(221) 

plot(time,-thl(l,:),'b');  xlabel('time  (s)');  title('Parameter  al');  grid 
subplot(222) 

plot(time,thl(2,:),'b');  xlabel('time  (s)');  title('Parameter  a2');  grid 
subplot(223) 

plot(time,-tli2(l,;),'b');  xlabel('time  (s)');  title('Parameter  b  T);  grid 
subplot(224) 

plot(time,th2(2,:),'b');  xlabel('time  (s)');  title('Parameter  b2');  grid 
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APPENDIX  B.  SONAR  CHARACTERISTICS 


The  primary  sonar  currently  installed  aboard  the  NPS  Phoenix  AUV  is  the  Tritech 
ST  1000  Profiling  System.  This  profiling  sonar  is  a  high  performance,  high  resolution 
compact  and  lightweight  system  which  produces  accurate  underwater  measurements.  The 
following  list  provides  a  summary  of  the  sonar  performance  characteristics  and 
specifications: 

Frequency . 1  MHz 

Beamwidth . 0.9  degree  conical 

Maximum  Range . 50  meters  (160  feet) 

Minimum  Range . 0-3  meters  (1  foot) 

Weight . 1-5  kg 

View  Sector . 360  degrees 

Scan  Rate . 40  degrees/second 

These  parameters  were  utilized  for  producing  simulated  sonar  data  in  all  theoretical 
simulations  conducted  for  this  thesis. 
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APPENDIX  C.  MATLAB  AND  SIMULINK  FILES 


A.  GENERAL 

This  appendix  contains  the  files  generated  utilizing  the  MATLAB  with  SIMULINK 
software  package.  The  programs  are  separated  by  section  for  each  Chapter  of  the  thesis  main 
body.  Each  program  is  headed  with  a  short  description  of  its  function  and  usage  and  includes 
appropriate  comments  for  documentation.  In  general,  each  type  of  scenario  is  simulated  with 
a  controlling  program  named  MAIN****.m  with  the  asterisks  replaced  by  a  descriptive 
abbreviation  such  as  "OPEN"  for  an  open  environment  simulation.  All  functions  which 
calculate  sonar  data  are  named  SONAR****.m,  potential  function  calculation  programs  are 
listed  as  VG****.m,  etc.  with  the  asterisks  being  the  same  for  all  associated  subprograms. 

B.  PROGRAM  LISTINGS  FOR  SIMULATIONS  IN  CHAPTER  H 

%%%%  MATLAB  Script  File  CONVTEST.m  is  utilized  to  verify  the  convergence 
%%%%  of  the  navigation  algorithm  in  a  simple  one-dimensional  scenario 
%%%%  for  two  cases  of  1)  Velocity  measured  and  2)  Acceleration  measured. 
%%%% 


%  Specify  Simulation  Parameters 

Ts=0.0225;  %  Step  Size 

time=Ts  *  (0 ;  1 600);  %  Generate  time  vector 

xact=3  ;0.2*Ts;  10.2;  %  Generate  actual  vehicle  position 

lamda=0.25;  %  Describe  potential  function 

%  Case  1:  Measured  Velocity 

mu  1 = 1 0;  %  Position  rate  estimation  gain 

vact=0.2;  %  Actual  vehicle  velocity  (fps) 

xestl=zeros(l,1601);  %  Initialize  output  vector 
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xestl(l)=4;  %  Initial  position  estimate  (ft) 

phatdot=zeros(  1 , 1 60 1 );  %  Initialize  position  rate  estimate  vector 

%  Iterative  Algorithm 
fork-2:1601 

s=(12-xact(k-l))*(l+0.05*randn(l,  1));  %  Sonar  data  w/  noise 

vwn=vact*(l+0.05*randn(l,l));  %  Velocity  measurement  w/  noise 

shat=xestl  (k- 1  )+s;  %  Position  of  tip  of  sonar  vector 

[v,dvx]=vgtest(shat,lamda);  %  Calculate  value  of  potential  fen 

phatdot(k)=vwn+mul  *v*dvx;  %  Estimate  position  rate 

xestl(k)=xestl (k-l)+Ts*phatdot(k);  %  Update  position  estimate 

end 

%  Generate  Output 
clg 

plot(time,xact,'b-.') 

grid 

hold 

plot(time,xestl,'b') 

axis([0  36  0  12]) 

xlabel('time  (sec)') 

ylabel('x-coordinate  (ft)') 

title('Position  Estimate  w/  Measured  Velocity') 

pause 

%  Case  2:  Measured  Acceleration 

mu2=10;  %  Velocity  rate  estimation  gain 

xest2=zeros(l,1601);  %  Initialize  output  vector 
xest2(l)=4;  %  Initial  position  estimate  (ft) 

a=0;  %  Vehicle  Acceleration  (ft/s'^2) 

vhat=zeros(l,  1601);  %  Initialize  velocity  estimate  vector 
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vhat(l)=0;  %  Initial  velocity  estimate  (fps) 

vhatdot=zeros(  1,1601);  %  Initialize  velocity  rate  estimate  vector 

%  Iterative  Algorithm 
for  n=2:1601 

s=(12-xact(n-l))*(l+0.05*randn(l,l));  %  Sonar  data  w/noise 

shat=xest2(n-l)+s;  %  Position  of  tip  of  sonar  vector 

[v,dvx]=vgtest(shat,lamda);  %  Calculate  value  of  potential  fen 

vhatdot(n)=a+mu2*v*dvx;  %  Estimate  velocity  rate 

vhat(n)=vhat(n-l)+Ts*vhatdot(n);  %  Update  velocity  estimate 
phatdot(n)=vhat(n)+mu  1  *v*dvx;  %  Estimate  position  rate 

xest2(n)=xest2(n-l)+Ts*phatdot(n);  %  Update  position  estimate 

end 

%  Generate  Output 
clg 

subplot(21 1) 

plot(time,xact,'b-.') 

grid 

hold 

plot(time,xest2,'b') 
axis([0  36  0  12]) 
xlabel('time  (sec)') 
ylabel('x-coordinate  (ft)') 

title('Position  Estimate  w/  Measured  Acceleration') 
subplot(212) 

plot(time,vact*ones(l ,  1 601),'b-.') 

grid 

hold 

plot(time,vhat,'b') 
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axis([0  36  -0.4  0.4]) 
xlabel('time  (sec)') 
ylabelf  velocity  (fps)') 

%%%%  MATLAB  script  file  CORRCALC.m  assists  the  user  in  determining  an 

%%%%  an  appropriate  value  for  both  lamda  and  mu  by  showing  the  value 

%%%%  of  the  navigation  correction  factor  and  the  width  of  the  attractive 

%%%%  field  in  either  the  x  or  y-axis  direction. 

%%%% 


%  Prompt  User  for  Input  Data 
lamda=input('Enter  value  of  lamda:  '); 
mu=input('Enter  value  of  mu;  '); 

flag=input('Enter  "1"  if  x-correction  desired  or  "2"  if  y-correction:  '); 
if  flag  =1 

val=input('Enter  y-coordinate  (ft);  '); 
else 

val=input('Enter  x-coordinate  (ft):  '); 
end 

begin=input('Enter  start  of  range  of  interest  (ft):  '); 
stop=input('Enter  end  of  range  of  interest  (ft):  '); 
range=begin:  0. 02:stop; 
long=length(range); 

%  Calculate  Correction  Factors 
for  k=l:long 
if  flag  =  1 

[v,dvx,dvy]=vgrad(range(k),val, lamda); 
corr(k)=mu*v*dvx; 
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else 

[v,  dvx,  dvy  ]=vgrad(val,range(k),  lamda); 
corr(k)=mu*v*dvy; 
end 
end 

%  Plotting  Commands 
plot(range,corr) 
xlabel('feet') 
y  lab  el('magnitude') 

title('Navigation  Correction  Factor  Magnitude  (One-dimension)') 

C.  PROGRAM  LISTINGS  FOR  SIMULATIONS  IN  CHAPTER  m 

%%%%  MATLAB  File  AUVPOSIT.M  prompts  the  user  for  simulation  parameters 
%%%%  and  then  utilizes  a  state-space  model  to  determine  actual  AUV 
%%%%  data  for  the  length  of  the  simulation.  The  output  state  vector 
%%%%  consists  of  x-coordinate,  y-coordinate,  velocity,  hdg  (in  degrees), 

%%%%  and  heading  rate. 

%  Prompt  user  for  simulation  parameters 
Tf=input('Enter  length  of  simulation  (seconds):  '); 
xO=input('Enter  initial  x-coordinate  (ft);  '); 
yO=input('Enter  initial  y-coordinate  (ft):  '); 
thetaO=input('Enter  initial  course  (degrees):  '); 
tums=input('Enter  time  to  start  turn  (seconds);  '); 
rudang=input('Enter  rudder  angle  (degrees)(+  left,  -  right):  '); 
tume=input('Enter  time  to  stop  turn  (seconds):  '); 
vO=input('Enter  initial  vehicle  velocity  (ft/s):  '); 
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rpm=mput('Enter  commanded  motor  rpm;  '); 

Ll=input('Enter  x-coordinate  range  of  oparea  (ft):  '); 
L2=input('Enter  y-coordinate  range  of  oparea  (ft):  '); 
dispC '); 

Ts=0.0225;  %  Time  Step 

k=round(Tf/Ts)+ 1 ;  %  Number  of  Iterations 

state=zeros(5,k-l);  %  Initialize  State  Vector  (Matrix) 

state(:,I)=[xO  yO  vO  thetaO  0]';  %  Initialize  State  Vector 
hdg(l)=state(4,l); 

u=[rpm*ones(l,k);zeros(l,k)];  %  Initialize  Control  Input 
indexs=round(tums/Ts);  %  Index  when  Turn  Starts 

indexe=round(tume/Ts);  %  Index  when  Turn  Ends 

tlength=indexe-indexs+l ;  %  Length  of  Turn  in  #  Steps 

tu=rudang*ones(  1  ,tlength);  %  Rudder  Control  Input  During  Turn 
u(2,indexs :  indexe)=tu; 

%  Define  Coefficients  for  Time-Dependent  State-Space  Model 

alpha=508.4; 

beta=1.6677; 

gamma=l; 

delta=l; 

%  Run  State-Space  Model  for  Length  of  Simulation 
for  i=2:k 

a=cos(state(4,i- 1  )*pi/l  80); 
b=sin(state(4,i- 1  )*pi/ 1 80); 

A=[0  0  a  0  0;0  0  b  0  0;0  0  -alpha  0  0;0  0  0  0  1;0  0  0  0  -gamma]; 
B=[0  0;0  0;beta  0;0  0;0  delta]; 

[phi,del]=c2d(A,B,Ts); 

state(:,i)=phi*state(:,i-l)+del*u(:,i-l); 


hdg(i)=state(4,i); 

ifhdg(i)<0 

hdg(i)=hdg(i)+360; 

end 

end 

%  Resolve  Velocity  into  xy-components 

xvel=state(3, :).  *(cos((pi/ 1 80)*state(4, :))); 

yvel=state(3,:).*(sin((pi/180)*state(4,;))); 

%  Output/Plotting  Commands 

timvec=0:Ts:Tf; 

orient  landscape 

clg 

subplot(21 1) 
plot(timvec,state(  1 , :)) 
axis([0  Tf  0  LI]) 
grid 

title('x-coordinate  vs.  time') 

xlabel('time  -  sec') 

ylabel('feet') 

subplot(212) 

plot(timvec,state(2, :)) 

axis([0  Tf  0  L2]) 

grid 

title('y-coordinate  vs.  time') 

xiabel('time  -  sec') 

ylabel('feet') 

pause 

clg 
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subplot(21 1) 
plot(timvec,state(3, :)) 
axis([0  Tf  0  0.6]) 
grid 

title('Velocity  vs.  Time') 

xlabel('time  -  sec') 

ylabel('ft/sec') 

subplot(212) 

plot(timvec,hdg) 

axis([0  Tf  0  360]) 

grid 

title('Heading  vs.  Time  (Angle  referenced  to  x-axis)') 

xlabel('time  -  sec') 

ylabel('degrees') 

pause 

clg 

subplot(21 1) 
plot(timvec,state(5, ;)) 
axis([0Tf-12  12]) 
grid 

title('Heading  Rate  vs.  Time  (ccw  =  positive)') 

xlabel('time  -  sec') 

ylabel('degrees/sec') 

pause 

clg 

subplot(21 1) 
plot(timvec,u(  1 , :)) 
axis([0  Tf  0  150]) 


60 


grid 

title('Speed  (rpm)  Input  vs.  Time') 

xlabel('time  -  sec') 

ylabel('rpm') 

subplot(212) 

plot(timvec,u(2, ;)) 

axis([0  Tf-12  12]) 

grid 

title('Commanded  Rudder  Angle  Input  vs.  Time') 

xlabel('time  -  sec') 

ylabel('degrees') 

pause 

orient  portrait 
clg 

plot(state(  1 ,  :),state(2, :)) 
axis([0  LI  0  L2]) 
grid 

titIe('AUV  Actual  Track') 
xlabel('x-coordinate  -  feet') 
ylabel('y-coordinate  -  feet') 
pause 

%%%%  MATLAB  File  MAINSQ.M  is  the  main  program  which  prompts 

%%%%  the  user  for  the  parameters  to  be  utilized  in  the  navigation 

%%%%  algorithm  and  the  initial  position  estimate.  This  program 
%%%%  calls  several  supporting  programs  to  process  the  sonar 

%%%%  measurements  and  calculate  the  value  and  gradient  of  the  potential 
%%%%  function. 
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%  Prompt  user  for  simulation  parameters 
lamda=input('Enter  value  of  lamda  (50  recommended):  '); 
mu=input('Enter  value  of  mu  (5  recommended):  '); 
xhatO=input('Enter  initial  estimate  of  x-coordinate:  '); 
yhatO=input('Enter  initial  estimate  of  y-coordinate:  '); 

Ll=input('Enter  x-dimension  width  of  oparea  (ft):  '); 

L2=input('Enter  y-dimension  height  of  oparea  (ft):  '); 
dispC '); 

%  Initialize  algorithm 
global  LI  L2 

phat0=[xhat0;yhat0];  %  Initial  Position  Estimate 

phat=zeros(2,Tf/9*400);  %  Initialize  Output  Position  Matrix 

theta=0;  %  Initial  Sonar  Bearing 

phat(:,  l)=phat0; 

s0=sonar(state(l,l),state(2,l), theta);  %  Initial  Sonar  Vector 
temps=phat0+s0; 

border(:,  l)=temps;  %  Initial  Pool  Border  Estimate 

%  Run  Simulation 
for  n=2:k 

x=state(  1  ,n);  %  Actual  x-coordinate  of  AUV 

y=state(2,n);  %  Actual  y-coordinate  of  AUV 

s=sonar(x,y,theta);  %  Noise-corrupted  Sonar  Vector 
z0=[state(3,n);state(4,n)*pi/180];  %  Vehicle  Velocity  &  Heading 
zcurr=z0+[randn(l)*0.05*z0(l);randn(l)*0.5*pi/180];  %  w/  Error 

pdot=[zcurr(l)*cos(zcurr(2));zcurr(l)*sin(zcurr(2))];  %  vx,vy 
shat=phat(:  ,n- 1  )+pdot*Ts+s;  %  Estimated  Position  +  Sonar  Vector 

border(:,n)=shat;  %  Estimated  Border  Point  of  Pool 

%  Estimate  of  Pool  Border  Orientation 
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sangle=atan(abs(shat(2)-temps(2))/ abs(shat(  1  )-temps(  1 ))); 
temps=shat; 

%  Calculate  Value  and  Gradient  of  Potential  Function 
[v,  dvx,  dvy  ]=vgrad(shat(  1 ),  shat(2),  lamda) ; 

%  Apply  Correction  Factor  to  Update  x  and  y  velocity  estimates 
pdothat^pdot+mu*v*[dvx*sin(sangle);dvy*cos(sangle)]; 

%  Obtain  current  position  estimate 
phat( ;  ,n)=phat(  :,n-l)+Ts*p  dothat; 

theta=theta+0.9*pi/180;  %  Increment  Sonar  Head  Bearing 
if  theta  >=2*pi  %  Reset  theta  to  0  after  360  deg 

theta=0; 
end 
end 

%%%%  MATLAB  Function  VGRAD.M  calculates  the  value  of  the 
%%%%  potential  function  and  its  gradient  for  a  given  input 
%%%%  position  and  the  parameter  lamda. 

%%%% 

function  [v, dvx, dvy]=vgrad(x,y, lamda); 
global  LI  L2 

v0=(x.  *(x-L  l))*(y .  *(y-L2))';  %  Value  of  Potential  Function 

z=v0/lamda; 

v=sigmoid(z);  %  Apply  "Squashing"  Function 

dvx=-dsig(z)*((2*x-Ll)*(y.*(y-L2))')/lamda;  %  x-component  of  gradient 
dvy=-dsig(z)*((x.*(x-Ll))*(2*y-L2)')/lamda;  %  y-component  of  gradient 
end 
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%%%% 

%%%% 

%%%% 


MATLAB  Function  SIGMOID.M  calculates  the  value  of  the 
sigmoid  utilized  as  a  squashing  function. 


function  y=sigmoid(x) 
x=min(x,100); 
x=max(x,-100); 
y=(  1  -exp(-x))./(  1  +exp(-x)); 
end 


%%%%  MATLAB  Function  DSIG.M  calculates  the  derivative  of  the 
%%%%  sigmoid  function. 

%%%% 


function  d=dsig(x) 
x=min(x,  100); 
x=max(x,-100); 
temp=exp(-x); 

d=2*temp ./( 1  +2  *temp+temp.  *temp); 
end 


%%%%  MATLAB  File  WITHDATA.m  commands  the  navigation  algorithm  when 
%%%%  processing  actual  test  data  obtained  with  the  NFS  PHOENIX  AUV. 
%%%%  The  sonar  test  data  is  loaded  into  the  MATLAB  Workspace  as  a 
%%%%  matrix  variable  "data"  consisting  of  column  vectors  of  bearing 
%%%%  and  range.  The  test  data  in  ASCII  named  robl.d,  etc.  consists  of 
%%%%  measurements  taken  at  0.045  second  intervals  requiring  a  modification 
%%%%  to  the  regular  algorithm  integration  step  size. 
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%  Prompt  user  for  simulation  parameters 
lamda=input('Enter  value  of  lamda:  '); 
xhatO=mput('Enter  value  of  xhatO;  '); 
yhatO=input('Enter  value  of  yhatO:  '); 
mu=input('Enter  value  of  mu:  '); 

Ts=0.045;  %  Integration  step  size 

%  Initialize  algorithm 

phatO=[xhatO;yhatO];  %  Initial  position  estimate 

phat=zeros(2, 1201);  %  Initialize  output  position  est 

border=zeros(2, 1201);  %  Initialize  border 

%  Run  Simulation 

s0=[data(l,l);data(l,2)];  %  Initial  sonar  measurement 

temps=phat0+[s0(2)*cos(s0(l)*pi/180);s0(2)*sin(s0(l)*pi/180)]; 

border(;,l)=temps; 

phat(:,  l)=phat0; 

for  n=2: 1201 

if  n  >=  1000  %  If  reqd  increase  fen  width 

lamda=4000; 
mu=400; 
end 

s=[data(n,  l);data(n,2)];  %  Current  sonar  measurement 

shat=phat(:,n-l)+[s(2)*cos(s(I)*pi/180);s(2)*sin(s(l)*pi/180)]; 

border(;,n)=shat; 

%  Estimate  of  Pool  Border  Orientation 

sangle=atan(abs(shat(2)-temps(2))/abs(shat(l)-temps(l)+le-9)); 

temps=shat; 

%  Calculate  Value  and  Gradient  of  Potential  Function 
[v,dvx,dvy]=vgrad6(shat(l),shat(2), lamda); 
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%  Apply  Correction  Factor  to  Update  x  and  y  velocity  estimates 
pdothat=pdot+mu*v*[dvx*sin(sangle);dvy*cos(sangle)]; 

%  Obtain  current  position  estimate 
phat( : ,  n)=phat(  :,n-l)+Ts*pdothat; 
end 


%%%%  MATLAB  Function  VGRAD.M  calculates  the  value  of  the 
%%%%  potential  function  and  its  gradient  for  a  given  input 
%%%%  position  and  the  parameter  lamda  for  simulations  with 

%%%%  actual  sonar  test  data  in  the  6x6  meter  test  pool. 

%%%% 

function  [v,  dvx,  dvy]=vgrad(x,y, lamda); 

v0=(x.*(x-19.68))*(y.*(y-19.68))';  %  Value  of  Potential  Function 

z=vO/lamda; 

v=sigmoid(z);  %  Apply  "Squashing"  Function 

dvx=-dsig(z)*((2*x-l  9. 68)*(y.  *(y- 1 9.68))')/lamda;  %  x-component  of  gradient 
dvy=-dsig(z)*((x.*(x-19.68))*(2*y-19.68)')/lamda;  %  y-component  of  gradient 
end 


%%%%  MATLAB  File  MAINOB  S 1  .m  is  the  main  program  which  prompts 
%%%%  the  user  for  the  parameters  to  be  utilized  in  the  navigation 
%%%%  algorithm  and  the  initial  position  estimate  for  a  scenario  including 

%%%%  a  cylindrical  obstacle  modeled  in  the  potential  function.  This 

%%%%  program  calls  several  supporting  programs  to  process  the  sonar 
%%%%  measurements  and  calculate  the  value  and  gradient  of  the  potential 
%%%%  function. 

%%%% 
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%  Prompt  user  for  simulation  parameters 
lamda=input('Enter  value  of  lamda:  '); 
mu=input('Enter  value  of  mu;  '); 
xhatO=mput('Enter  initial  estimate  of  x-coordinate;  '); 
yhatO=input('Enter  initial  estimate  of  y-coordinate:  '); 
dispc '); 

%  Initialize  algorithm 

phatO=[xhatO;yhatO];  %  Initial  Position  Estimate 
phat=zeros(2,Tf/9*400);  %  Initialize  Output  Position  Matrix 
theta=0;  %  Initial  Sonar  Bearing 
phat(:,  l)=phatO; 

s0=sonobs(state(l,l),state(2,l), theta);  %  Initial  Sonar  Vector 

temps=phatO+[sO(  1  );s0(2)]; 

border(:,  l)=temps;  %  Initial  Pool  Border  Estimate 

%  Run  Simulation 

for  n=2:k 

x=state(  1  ,n);  %  Actual  x-coordinate  of  AUV 

y=state(2,n);  %  Actual  y-coordinate  of  AUV 

s=sonobs(x,y, theta);  %  Noise-corrupted  Sonar  Vector 

z0=[state(3,n);state(4,n)*pi/l 80];  %  Vehicle  Velocity  &  Heading 

zcurr=z0+[randn(l)*0.05*z0(l);randn(l)*0.5*pi/180];  %  w/  Error 

pdot=[zcurr(l)*cos(zcurr(2));zcurr(l)*sin(zcurr(2))];  %  vx,vy 
sha^hat(;,n-l)+pdot*Ts+[s(l);s(2)];  %  Estimated  Position  +  Sonar  Vector 
border(:,n)=shat;  %  Estimated  Border  Point  of  Pool 

%  Estimate  of  Pool  Border  Orientation 
ifs(3)=l 
sanglel=pi/2; 
sangle2=0; 


67 


else 

sangle  1  =atan(abs(shat(2)-temps(2))/abs(shat(  1  )-temps(  1 ))); 
sangle2=sanglel; 
end 

temps=shat; 

%  Calculate  Value  and  Gradient  of  Potential  Function 
[v,dvx,dvy]=vgcyl(shat(  1  ),sliat(2),lanida); 

%  Apply  Correction  Factor  to  Update  x  and  y  velocity  estimates 
pdothat=pdot+mu*v*(lamda/lamda0)*[dvx*sin(sanglel);dvy*cos(sangle2)]; 
%  Obtain  current  position  estimate 
phat( : ,  n)=phat( :  ,n- 1  )+T  s  *  pdothat; 

theta=theta+0.9*pi/180;  %  Increment  Sonar  Head  Bearing 

if  theta  >=2*pi  %  Reset  theta  to  0  after  360  deg 

theta=0; 
end 
end 

%%%%  MATLAB  Function  SONAR.M  calculates  the  sonar  range  and 
%%%%  bearing  to  the  rectangular  pool  border  given  the  position  in 
%%%%  the  pool  and  the  angle  of  the  sonar  head.  The  bearing 
%%%%  and  range  are  corrupted  with  noise  (error).  The  environment 
%%%%  includes  one  cylindrical  obstacle. 

%%%% 

function  s  =  sonobs(x,y, theta); 

global  yo  %  Obstacle  Description 

global  xo 
global  r 
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flag=0; 

%  Calculate  angles  from  position  to  4  pool  comers 
thetal=atan2(12-y,  12-x); 
theta2=atan2(x,  1 2-y)+pi/2; 
thetaS =atan2(y,x)+pi; 
theta4=atan2(12-x,y)+3  *(pi/2); 

%  Calculate  angle  to  obstacle 
thetobs=atan2(yo-y,xo-x); 
if  thetobs  <=  0 
thetobs=2*pi+thetobs; 
end 

robs=sqrt((xo-x)^2+(yo-y)^2); 
diff=atan(r/robs); 
if  (theta  >=  (thetobs-diff))  &  (theta  <=  (thetobs+diff)) 
flag=l; 
rho=robs-r; 
else 

if  theta  <=  theta  1  %  Calculate  Range 

rho=(  1 2-x)/cos(theta); 
elseif  theta  <=  theta2 
rho=(  1 2-y)/sin(theta); 
elseif  theta  <=  thetaS 
rho=x/cos(pi-theta); 
elseif  theta  <=  theta4 
rho=y/abs(cos(  1 . 5  *pi-theta)); 
else 

rho=(  1 2-x)/cos(theta); 
end 


%  Upper  Right  Comer 
%  Upper  Left  Comer 
%  Lower  Left  Comer 
%  Lower  Right  Comer 
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end 

rhon=rho*(l+0.0225*randn(l,  1));  %  Add  Range  Error 

thetan=theta+(0.45 *pi/l  80)*randn(  1,1);  %  Add  Bearing  Error 

s=[rhon*cos(thetan);rhon*sin(thetan);flag];  %  Convert  to  x,y  Components 

end 

%%%%  MATLAB  Function  VGCYL.M  calculates  the  value  of  the 
%%%%  potential  function  and  its  gradient  for  a  given  input 
%%%%  position  and  the  parameter  lamda  for  the  rectangular 
%%%%  environment  with  one  modelled  obstacle. 

function  [v,dvx,dvy]=vgcyl(x,y, lamda); 
global  xo  yo  r  %  Description  of  Obstacle 

a=xo; 
b=yo; 
c=r^2; 

v0=(x.*(x-12).*(x-a).^2)*(y.*(y-12))’+(x.*(x-12))*(y.*(y-12).*(y-b).^2)'-c*(x.*(x-12)) 

*(y.*(y-12))';  %  Value  of  Potential  Function 

z=vO/lamda; 

v=sigmoid(z);  %  Apply  "Squashing"  Function 

dvx=-dsig(z)*(((x-12).*(x-a).^2)*(y.*(y-12))'+(x.*(x-a).^2)*(y.*(y-12))'+(2*x.*(x-12). 
*(x-a))*(y.*(y-12))'+(x-12)*(y.*(y-12).*(y-b).^2)'+x*(y.*(y-12).*(y-b).^2)'-c*(x-12)* 
(y.*(y-12))'-c*x*(y.*(y-12))')/lamda;  %  x-component  of  gradient 
dvy=-dsig(z)*((x.  *(x-12).  *(x-a).^2)*(y- 1 2)’+(x.  *(x- 1 2).  *(x-a).^2)*y'+(x.  *(x-l  2))* 
((y-12).*(y-b).^2)’+(x.*(x-12))*(y.*(y-b).^2)'+(x.*(x-12))*(2*y.*(y-12).*(y-b))'-c* 
(x.*(x-12))*(y-12)'-c*(x.*(x-12))*y')/lamda;  %  y-component  of  gradient 
end 
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D.  PROGRAM  LISTINGS  FOR  SIMULATIONS  IN  CHAPTER  IV 


%%%%  MATLAB  File  MAINOPEN.m  implements  the  navigation  algorithm  in 

%%%%  the  open  environment  scenario.  It  prompts 

%%%%  the  user  for  the  parameters  to  be  utilized  in  the  navigation 

%%%%  algorithm  and  the  initial  position  estimate  (guess).  This  program 

%%%%  calls  several  supporting  programs  to  process  the  sonar 

%%%%  measurements  and  calculate  the  value  and  gradient  of  the  potential 

%%%%  function. 

%  Prompt  user  for  simulation  parameters 
lamda=input('Enter  value  of  lamda  (50  recommended):  '); 
mu=input('Enter  value  of  mu  (10  recommended):  '); 
xhatO=input('Enter  initial  estimate  of  x-coordinate  (ft):  ')i 
yhatO=input('Enter  initial  estimate  of  y-coordinate  (ft):  '); 
dispC '); 

%  Initialize  algorithm 

phat0=[xhat0;yhat0];  %  Initial  Position  Estimate 

phat=zeros(2,Tf/9*400);  %  Initialize  Output  Position  Matrix 

theta=0;  %  Initial  Sonar  Bearing 

phat(:,l)=phatO; 

s0=sonopen2(state(l,l),state(2,l), theta);  %  Initial  Sonar  Vector 
temps=phat0+[s0(  1 ) ;  s0(2)] ; 

border(:,  l)=temps;  %  Initial  Pool  Border  Estimate 

%  Run  Simulation 
for  n=2:k 

x=state(l,n);  %  Actual  x-coordinate  of  AUV 

y=state(2,n);  %  Actual  y-coordinate  of  AUV 
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s=sonopen2(x,y, theta);  %  Noise-corrupted  Sonar  Vector 

z0=[state(3,n);state(4,n)*pi/l  80];  %  Vehicle  Velocity  &  Heading 

zcurr=z0+[randn(l)*0.05*z0(l);randn(l)*0.5*pi/180];  %  w/  Error 

pdot=[zcurr(l)*cos(zcurr(2));zcurr(l)*sin(zcurr(2))];  %  vx,vy 
shat=phat(:,n-l)+pdot*Ts+[s(l);s(2)];  %  Estimated  Position  +  Sonar  Vector 
border(:,n)=shat;  %  Estimated  Border  Point  of  Pool 

temps=shat; 

%  Calculate  Value  and  Gradient  of  Potential  Function 
[v,dvx,dvy]=vgopen(shat(l),shat(2),lamda); 

%  Apply  Correction  Factor  to  Update  x  and  y  velocity  estimates 
pdothat=pdot+mu*  V*  [dvx;dvy] ; 

%  Obtain  current  position  estimate 
phat(:,n)=phat(:,n-l)+Ts*pdothat; 

theta=theta+0.9*pi/l 80;  %  Increment  Sonar  Head  Bearing 

if  theta  >=2*pi  %  Reset  theta  to  0  after  360  deg 

theta=0; 
end 
end 

%%%%  MATLAB  Function  SONAR.M  calculates  the  sonar  range  and 
%%%%  bearing  for  the  open  environment  where  navigation  corrections 
%%%%  are  derived  from  two  markers  (obstacles)  place  in  the  environment. 
%%%% 


function  s  =  sonobs(x,y, theta); 

global  xol  yol  xo2  yo2  rl  r2  %  Description  of  obstacles 

%  Calculate  angles  to  obstacles 
thetobs  1  =atan2(yo  1  -y,xo  1  -x); 
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thetobs2=atan2(yo2-y,xo2-x); 
if  thetobsl  <=  0 
thetobs  1  =2*pi+thetobs  1 ; 
end 

if  thetobs2  <=  0 
thetobs2=2  *pi+thetobs2; 
end 

robs  1  =sqrt((xo  1  -x)^2+(yo  1  -y)^2);  %  Range  to  ob  stacle  1 

robs2=sqrt((xo2-x)^2+(yo2-y)^2);  %  Range  to  obstacle  2 

diff  1  =atan(r  1  /robs  1 ); 
dif£2=atan(r2/robs2); 

if  (theta  >=  (thetobs  1-difFl))  &  (theta  <=  (thetobs  1+diffl)) 
rho=robsl-rl; 

elseif  (theta  >=  (thetobs2-diff2))  &  (theta  <=  (thetobs2+diff2)) 
rho=robs2-r2; 
else 

rho=500;  %  Assign  large  value  if  not  on  obstacle 

end 

rhon=rho*(l+0.01*randn(l,  1));  %  Add  Range  Error 

thetan=theta+(0.45*pi/180)*randn(l,l);%  Add  Bearing  Error 
s=[rhon*cos(thetan);rhon*sin(thetan)];%  Convert  to  x,y  Components 
end 

%%%%  MATLAB  Function  VGOPEN.M  calculates  the  value  of  the 
%%%%  potential  function  and  its  gradient  for  a  given  input 
%%%%  position  and  the  parameter  lamda  for  the  open  environment 
%%%%  scenario  including  two  obstacles. 
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%%%% 


function  [v,dvx,dvy]=vgopen(x,y,lamda); 
global  xol  yol  rl  xo2  yo2  r2  %  Obstacle  description 

a=xol; 
b=yol; 
c=t1^2; 
d=xo2; 
e=yo2; 
f=r2^2; 

v0=((x-a).^2+(y-b).'^2-c).*((x-d).^2+(y-e).^2-f); 

z=vO/lainda; 

v=sigmoid(z);  %  Apply  "Squashing"  Function 

dvx=-dsig(z)*((2*(x-a)).*((x-d).^2+(y-e).^2-f)+((x-a).^2+(y-b).^2-c).*(2*(x-d)))/lamda; 
%  x-component  of  gradient 

dvy=-dsig(z)*((2*(y-b)).*((x-d).^2+(y-e)."^2-f)+((x-a),"^2+(y-b).^2-c).*(2*(y-e)))/lanida; 
%  y-component  of  gradient 
end 

%%%%  MATLAB  File  MODELSIM.m  is  the  main  program  which  implements  the 
%%%%  navigation  algorithm  for  a  complex  scenario  where  the  environment 
%%%%  is  comprised  of  multiple  components. 

%%%% 


%  Prompt  user  for  simulation  parameters 
lamda=input('Enter  value  of  lamda:  '); 
mu=input('Enter  value  of  mu:  '); 

xhatO=input('Enter  initial  estimate  of  x-coordinate  (ft);  '); 
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yhatO=input('Enter  initial  estimate  of  y-coordinate  (ft);  '); 
dispC '); 

%  Initialize  algorithm 

phatO=[xhatO;yhatO];  %  Initial  Position  Estimate 

phat=zeros(2,Tf/9*400);  %  Initialize  Output  Position  Matrix 

theta=0;  %  Initial  Sonar  Bearing 

phat(:,l)=phatO; 

s0=sonmodel(state(l,l),state(2,l),theta);  %  Initial  Sonar  Vector 
temps=phatO+[sO(  1  );s0(2)]; 

border(:,  l)=temps;  %  Initial  Pool  Border  Estimate 

%  Run  Simulation 
for  n=2:k 

x=state(l,n);  %  Actual  x-coordinate  of  AUV 

y=state(2,n);  %  Actual  y-coordinate  of  AUV 

s=sonmodel(x,y,theta);  %  Noise-corrupted  Sonar  Vector 

z0=[state(3,n);state(4,n)*pi/180];  %  Vehicle  Velocity  &  Heading 

zcurr=z0+[randn(l)*0.05*z0(l);randn(l)*0.5*pi/180];  %  w/  Error 

pdot=[zcurr(l)*cos(zcurr(2));zcurr(l)*sin(zcurr(2))];  %  vx,vy 
shat=phat(;,n-l)+pdot*Ts+[s(l);s(2)];  %  Estimated  Position  +  Sonar  Vector 
border(;,n)=shat;  %  Estimated  Border  Point  of  Environment 
temps=shat; 

%  Calculate  Value  and  Gradient  of  Potential  Function 
xindex=TOund(shat(l)/0.  l)+40; 
yindex=round(shat(2)/0.  l)+40; 
if  xindex  >=  281 
xindex=281; 
end 

if  yindex  >=  281 
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yindex=281; 

end 

if  xindex  <=1 
xindex=l; 
end 

if  y  index  <=1 
yindex=l; 
end 

%  wal=v(xindex,yindex); 

%  dvxval=dvx(xindex,yindex); 

%  dvyval=dvy(xindex,yindex); 

[  wal,  dvxval,  dvyval]=vgmodel(shat(  1 ),  shat(2),  lamda) ; 

%  Apply  Correction  Factor  to  Update  x  and  y  velocity  estimates 
pdothat=pdot+mu*  wal  *  [dvxval  *sin(s(3 ));  dvyval  *  cos(s(3 ))] ; 

%  Obtain  current  position  estimate 
phat(:,n)=phat(:,n-l)+Ts*pdothat; 

tlieta==theta+0.9*pi/l  80;  %  Increment  Sonar  Head  Bearing 

if  theta  >=2*pi  %  Reset  theta  to  0  after  3  60  deg 

theta=0; 
end 
end 

%%%%  MATLAB  Function  SONMODEL.m  calculates  the  sonar  range 
%%%%  and  bearing  to  the  complex  environment  consisting  of 
%%%%  multiple  components  given  the  position  and  orientation  of 
%%%%  the  sonar  head  mounted  on  the  vehicle. 

%%%% 
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function  s=sonmodel(x,y, theta) 

%  Calculate  bearing  to  each  intersection  of  model  components 
thetal  =atan2(  1 6-y,20-x); 
theta2=atan2(20-y,  1 6-x), 
ify>  10 

theta3=atan2(10-y,5-x)+2*pi; 

else 

thetaS =atan2(  1 0-y,  5  -x); 
end 

theta4=atan2(y,x)+pi; 
theta5=atan2(20-x,y)+3  *pi/2; 
rob3  =sqrt((5-x)''2+(  1 0-y)''2); 
diff3=atan(0,2/rob3); 

%  Calculate  range  to  component  sonar  beam  is  hitting 
if  theta  <=  thetal 
rho=(20-x)/cos(theta); 
sangle=pi/2; 
elseif  theta  <=theta2 

rho=(20-x)/cos(theta).(((20-x)nan(theta)-(16-y))*sin(pi/4))/sin(pi/4+theta); 

sangle=pi/4; 

elseif  (theta  >=  (theta3-diff3))  &  (theta  <=  (theta3+diff3)) 
rho=rob3-0.2; 
sangle=pi/4, 

elseif  (theta  >=  theta4)  &  (theta  <=  thetaS) 

rho=y/abs(cos(l  .5*pi-theta)); 

sangle=0; 

elseif  theta  >=  thetaS 
rho=(20-x)/cos(theta); 
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%  Range  +  error 
%  Bearing  +  error 


sangle=pi/2; 
else 

rho=50; 
sangle=0; 
end 

rhon=rho  *  ( I  +0 . 022 5  *  r  andn(  1 , 1 )); 
thetan=theta+(0-45*pi/l  80)*randn(l ,  1); 
s=[rhon*cos(thetan);rhon*sin(thetan);sangle];  %  Sonar  return 
end 

%%%%  MATLAB  Function  VGMODEL.m  calculates  the  value  of  the 
%%%%  potential  function  and  its  gradient  for  the  complex 
%%%%  environment  scenario. 

%%%% 


function  [v,  dvx,  dvy]=vgmodel(x,  y ,  lamda) 
v0=y.*(x-20).*((y-16)+(x-16)-4).*((x-5).'^2+(y-10).^2-0.2^2); 
z=v0/lamda; 
v=sigmoid(z); 

dvx=-ds ig(z) *  (((y- 1 6)+(x- 1 6)-4) .  ((x- 5 ) . ^2+(y- 1 0) . ^2-0 . 2'^2)+y .  * (x-20) .  *  ((x-5) .  2+ 
(y- 1  0).^2-0.2^2)+(2-(x-5)).  *y .  *(x-20).  *((y-l  6)+(x-1  6)-4))/lamda; 
dvy=-dsig(z)*((x-20).  *((y- 1 6)+(x-l  6)-4).  ='((x-5).^2+(y-  10).^2-0.2^2)+y.  ''(x-20).  =*= 
((x-5).^2+(y-10).^2-0.2'^2)+(2''(y-10)).*y.*(x-20).='((y-16)+(x-16)-4))/lamda; 
end 


78 


E. 


PROGRAM  LISTINGS  FOR  SIMULATIONS  IN  CHAPTER  V 


%%%%  MATLAB  script  file  SLEDEOBS.m  implements  a  sliding  mode 

%%%%  observer  for  a  simple  one-dimensional  scenario  as  a  proof 

%%%%  of  concept. 

%%%% 


x0=2; 

kl=2; 

k2=4; 

xlhat=3; 

x2hat=0.0; 

v=0.2; 

Ts=0.0225; 

Tf=36; 

k=(Tf/Ts)+l; 

xhat=zeros(2,k); 

xhat( ; ,  1  )=[x  1  hat;x2hat] ; 

xact=zeros(2,k); 

xact(;,l)=[xO;v]; 

time=zeros(l,k); 

%  Iterative  Algorithm 
for  i=2:k 


%  Initial  actual  position 
%  Sliding  mode  gains 

%  Initial  position  estimate 
%  Initial  velocity  estimate 
%  Actual  velocity 
%  Integration  step  size 

%  Length  of  simulation 
%  Index 

%  Initialize  estimate  vector 
%  Initial  estimate  x  v 
%  Initial  state  vector 
%  Initial  conditions 
%  Initialize  time  vector 


rhon=(12-xact(l,i-l))*(l+0.0225*randn(l,l));  %  Sonar  range  w/  error 

vn=v*(l+0.0225*randn(l,l));  %  Velocity  measurement 

%  Error  and  rate  calculations 
X 1  tilda=xhat(  1 ,  i- 1 )-( 1 2-rhon); 
x2tilda=xhat(2,  i- 1  )-vn; 
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xltildot=x2tiIda-kl*sign(xltilda); 
x2tildot=-k2  *  sign(x  1  tilda); 

X 1  hatdot=vn+x  1  tildot; 
x2hatdot=x2tildot; 

%  Update  position  estimate 
xhat(:,i)^hat(:,i-l)+[xlhatdot;x2hatdot]*Ts; 

%  Udate  actual  position  and  time 
xact(:,i)=[xact(l,i-l)+v*Ts;v]; 
time(i)=time(i- 1  )+T  s; 
end 

%%%%  MATLAB  script  file  SMOBS.m  implements  the  sliding  mode 
%%%%  observer  for  the  closed  border  rectangular  environment 
%%%%  scenario.  It  calls  the  function  SONARSM.m  to  generate 

%%%%  the  sonar  data.  Vehicle  data  is  generated  ivith  file 
%%%%  AUVDATA.m. 

%%%% 

%  Generate  vehicle  trajectory  in  component  directions 

xact=[state(l,;);state(3,;).*cos(state(4,:)*pi/180)]; 

yact=[state(2,:);state(3,:).*sin(state(4,;)*pi/180)]; 

%  User  input  of  initial  state  estimates 
xlhat=input('Enter  Initial  Estimate  of  x-coordinate:  '); 
ylhat=input('Enter  Initial  Estimate  of  y-coordinate:  '); 
x2hat=input('Enter  Initial  x-component  Velocity  Estimate;  '); 
y2hat=input('Enter  Initial  y-component  Velocity  Estimate:  '); 

dispC  '); 

Ts=0.0225;  %  Integration  step  size 
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k=length(state(  1 , %  Data  length 
kl=2;  %  Observer  gains 

k2=4; 

phat0=[xlhat;x2hat;ylhat;y2hat];  %  Initial  estimates 
phat=zeros(4,k);  %  Initialize  estimate  vector 

border=zeros(2,k- 1 );  %  Initialize  border  vector 

phat(:,  l)=phatO;  %  Initial  conditions 

theta=0;  %  Initial  sonar  bearing 

%  Iterative  Algorithm 
for  n=2;k 

s=sonarsm(xact(l,n-l),yact(l,n-l), theta);  %  Sonar  return  w/  error 
flag=s(3); 

pdot=[xact(2,n-l)*(l+0.0225*randn(l));yact(2,n-l)*(l+0.0225*randn(l))]; 
shat=[phat(l,n-l);phat(3,n-l)]+[s(l);s(2)];  %  Tip  of  sonar  vector 
border( ;  ,n- 1  )=shat;  %  Estimated  position  of  border 

%  Update  x-coordinate  components  -  return  on  right  side  of  pool 
if  flag  =  1 

X 1  tilda=phat(  1  ,n- 1  )-(20-s(  1 )); 
x2tilda=phat(2,n- 1  )-pdot(  1 ); 
xltildot=x2tilda-kl  *sign(xltilda); 
x2tildot=-k2*sign(xltilda); 

X 1  hatdot=pdot(  1  )+x  1  tildot; 
x2hatdot=x2tildot; 

xhat(:,n)=phat(l:2,n-l)+[xlhatdot;x2hatdot]*Ts; 

%  Update  state  estimates 

phat(:,n)=[xhat(;,n);phat(3,n-l)+phat(4,n-l)*Ts;phat(4,n-l)]; 

end 

%  Update  y-coordinate  components  -  return  on  top  side  of  pool 
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if  flag  =  2 

yltilda=phat(3,n-l)-(20-s(2)); 
y2tilda=phat(4,n- 1  )-pdot(2); 
yltildot=y2tilda-kl*sigii(y  Hilda); 
y2tildot=-k2  *  sign(y  Hilda); 
y  1  hatdot=pdot(2)+y  1  tildot; 
y2hatdot=y2tildot; 

yhat(:,n)=phat(3  ;4,n- 1  )+[y  1  hatdot;y21iatdot]  *Ts; 

%  Update  state  estimates 

phat(:,n)=[phat(l,n-l)+phat(2,n-l)*Ts;phat(2,n-l);yhat(;,n)]; 

end 

%  Update  x-coordinate  components  -  return  on  left  side  of  pool 
if  flag  =  3 

X 1  tilda=phat(  1  ,n- 1  )-ab  s(s(  1 )) ; 
x2tilda=phat(2,n- 1  )-pdot(  1 ); 

X 1  tildot=x2tilda-k  1  *  sign(x  1  tilda); 
x2tildot=-k2*sign(xHilda); 

X 1  hatdot=pdot(  1  )+x  1  tildot; 
x2hatdot=x2tildot; 

xhat( :  ,n)=phat(  1 : 2,n- 1  )+[x  1  hatdot;x2hatdot]  *Ts; 

%  Update  state  estimates 

phat(:,n)=[xhat(:,n);phat(3,n-l)+phat(4,n-l)*Ts;phat(4,n-l)]; 

end 

%  Update  y-coordinate  components  -  return  on  bottom  side  of  pool 
if  flag  =  4 

y  1  tilda=phat(3 ,  n- 1  )-ab  s(s(2)) ; 
y2tilda=pliat(4,n- 1  )-pdot(2); 
y  1  tildot=y2tilda-kl  *sign(y  1  tilda); 
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y2tildot=-k2*sign(y  1  tilda); 
y  1  hatdot=pdot(2)+y  Itildot; 
y2hatdot=y2tildot; 

yhat(:,n)=phat(3 :4,n- 1  )+[y  1  hatdot;y2hatdot]*Ts; 

%  Update  state  estimates 

phat(:,n)=[phat(l,n-l)+phat(2,n-l)*Ts;phat(2,n-l);yhat(;,ii)]; 

end 

theta=theta+0.9*pi/l 80;  %  Increment  sonar  bearing 

if  theta  >=2*pi  %  Reset  to  zero  after  360  deg  scan 

theta=0; 
end 
end 


%%%% 

%%%% 

%%%% 

%%%% 

%%%% 

%%%% 

%%%% 


MATLAB  Function  SONARSM.m  calculates  the  sonar  range  and 

bearing  to  the  square  pool  border  given  the  position  in 

the  pool  and  the  angle  of  the  sonar  head.  The  bearing 

and  range  are  corrupted  with  noise  (error).  A  flag  is  returned 

to  the  calling  program  to  indicated  which  side  of  the  pool 

the  sonar  beam  is  hitting. 


function  s  =  sonarsm(x,y, theta); 

%  Calculate  angles  from  position  to  4  pool  comers 
thetal=atan2(20-y,20-x);  %  Upper  Right  Comer 
theta2=atan2(x,20-y)+pi/2;  %  Upper  Left  Comer 
theta3=atan2(y,x)+pi;  %  Lower  Left  Comer 
theta4=atan2(20-x,y)+3*(pi/2);  %  Lower  Right  Comer 
if  theta  <==  theta  1 


83 


rho=(20-x)/cos(theta); 

flag=l; 

elseif  theta  <=  theta2 
rho=(20-y)/sin(theta); 
flag=2; 

elseif  theta  <=  thetaS 
rho=x/cos(pi-theta); 
flag=3; 

elseif  theta  <=  theta4 
rho=y/abs(cos(l  .5*pi-theta)); 
flag=4; 
else 

rho=(20-x)/cos(theta); 

flag=l; 

end 

rhon=rho*(l+0.0225*randn(l,l));  %  Add  Range  Error 

thetan=theta+(0.45*pi/180)*randn(l,l);  %  Add  Bearing  Error 
s=[rhon*cos(thetan);rhon*sin(thetan);flag];  %  Convert  to  x,y  Components 
end 

F.  AUV  SIMULINK  MODEL 

In  addition  to  the  MATLAB  script  file  AUVDATA.m  the  SIMULINK  model 
AUVMODEL.m  can  be  utilized  to  generate  the  complete  state  trajectory  of  the  AUV  for  any 
simulation.  This  model  can  account  for  the  motion  resulting  from  two  commanded 
maneuvers  during  the  vehicle  run.  The  model  is  presented  as  Figure  C.l. 
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Figure  C.l  SIMULINK  Model  AUVMODEL.in 
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APPENDIX  D.  SMOOTHING  FILTER  DESIGN 


The  design  of  digital  filters  is  extensively  covered  in  the  technical  literature.  The 
choice  of  a  smoothing  filter  for  the  purpose  of  this  thesis  follows  the  principles  outlined  in 
[Ref  8], 

A  logical  choice  in  determining  a  strategy  for  filtering  (or  smoothing)  the  AUV 
estimated  position  data  is  to  base  the  filter  output  on  an  average  of  the  parameter  data 
measured  over  the  previous  sonar  scan  (i.e.  400  data  points).  If  we  consider  x(n)  as  the  input 
data  sequence  and  y(n)  as  the  output  of  the  filter  the  following  difference  equation  is 
obtained: 


y(n) 


400 


•(jc(«)  +  ...  t  x(n  -  400  +  1)) 


(D.l) 


Taking  the  z-transform  of  this  difference  equation  yields 


399 


Hi^)  =  —  E  - 

400  * . 0 


1  -  z 


-400 


1-  Z 


(D.2) 


Now,  application  of  the  inverse  z-transform  to  the  simplified  system  function  of 
Equation  (D.2)  yields 

y(n)  =  y(n  -  1)  +  — ^(jc(/i)  -  x(n  -  400))  (D.3) 

400 

which  simply  states  that  the  filter  output  is  the  previous  output  plus  a  weighted  sum  of  the 
change  in  data  over  the  previous  sonar  scan.  The  SIMULINK  Model  of  the  filter  is  presented 
as  Figure  D.  1  and  is  followed  by  the  MATLAB  script  file  FILTXY.m  which  formats  the 
input  and  commands  operation  of  the  model.  The  filter  is  initialized  with  a  vector  of  400 


87 


components  equal  to  the  initial  position  estimate.  An  accurate  filtered  output  value  is  not 
available  until  the  first  sonar  scan  is  completed  (400  measurements). 


SIMULATION  PARAMETERS 
RK4-5 

Min  Step  0.0001 
Max  Step  0.0225 
Tolerance  0.001 


Clock 


Figure  D.l  SIMULINK  Model  of  Smoothing  Filter 


%%%%  MATLAB  File  FELTXY.M  filters  the  x  and  y  estimates  of 
%%%%  AUV  position  utilizing  a  SIMULINK  Model  AUVFILT.M. 
%%%% 


xsize=size(phat);  %  Determine  data  length 

long=xsize(2); 

yinx=[xhat0*ones(  1,400)  phat(l,:)];  %  Initialize  filter  input  x 
ymy=[yhat0*ones( 1,400)  phat(2,:)];  %  Initialize  filter  input  y 
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realt=0:Ts:Tf; 
tindex=0 :  Ts :  Tf+9 ; 
ym=[tindex'  yinx']; 
dummy=rk45('auvfilt',[0  Tf+9],[ 
clear  dummy 

filtx=yout(600 ;  long+3 99); 
clear  yout 
yin=[tindex'  yiny']; 
dummy=rk45('auvfilt',[0  Tf+9],[ 
clear  dummy 

filty=yout(600:long+399); 


%  Real  time  vector 
%  Data  input  time  vector 
%  Filter  input  x-coordinate 
],[Ts,Ts,le-3]);  %  Run  filter 

%  Generate  filtered  x  output 
%  Clear  model  output 
%  Filter  input  y-coordinate 
],[Ts,Ts, le-3]);  %  Run  filter 

%  Generate  filtered  y  output 
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